Управление с джойстика через python

Нет ответов
OK
Offline
Зарегистрирован: 23.02.2014

Задача очень простая:

Есть рабочий код как на ардуино так и на pythonе, нужно исправить-добавить в код: что бы две оси X и Y

дружили вместе т.е.  едем в перед ось X, работают два мотора-серво, а при повороте по оси Y один мотор-серво замедлял ход пропорцианально отклонению оси Y.

имитация движения танка.

Код ардуино:

001/* ------------------------------
002 *   MultipleSerialServoControl
003 * ------------------------------
004 *
005 * Uses the Arduino Serial library
007 * and the Arduino Servo library
009 * to control multiple servos from a PC using a USB cable.
010 *
011 * Dependencies:
012 *   Arduino 0017 or higher
014 *   Python servo.py module
016 *
017 * Created:  23 December 2009
018 * Author:   Brian D. Wendt
020 * Version:  1.1
021 * License:  GPLv3
023 *
024 */
025 
026// Import the Arduino Servo library
027#include <Servo.h>
028 
029// Create a Servo object for each servo
030Servo servo1;
031Servo servo2;
032Servo servo3;
033Servo servo4;
034// TO ADD SERVOS:
035//   Servo servo5;
036//   etc...
037 
038// Common servo setup values
039int minPulse = 600;   // minimum servo position, us (microseconds)
040int maxPulse = 2400;  // maximum servo position, us
041 
042// User input for servo and position
043int userInput[3];    // raw input from serial buffer, 3 bytes
044int startbyte;       // start byte, begin reading input
045int servo;           // which servo to pulse?
046int pos;             // servo angle 0-180
047int i;               // iterator
048 
049 
050// LED on Pin 13 for digital on/off demo
051int ledPin = 13;
052int pinState = LOW;
053 
054void setup()
055{
056  // Attach each Servo object to a digital pin
057  servo1.attach(2, minPulse, maxPulse);
058  servo2.attach(3, minPulse, maxPulse);
059  servo3.attach(4, minPulse, maxPulse);
060  servo4.attach(5, minPulse, maxPulse);
061  // TO ADD SERVOS:
062  //   servo5.attach(YOUR_PIN, minPulse, maxPulse);
063  //   etc...
064 
065  // LED on Pin 13 for digital on/off demo
066  pinMode(ledPin, OUTPUT);
067 
068  // Open the serial connection, 9600 baud
069  Serial.begin(9600);
070  pinMode(7,OUTPUT);
071  digitalWrite(7,LOW);
072  Serial.println("Slave Ready");
073}
074 
075void loop()
076{
077  // Wait for serial input (min 3 bytes in buffer)
078  if (Serial.available() > 2) {
079    // Read the first byte
080    
081    
082    startbyte = Serial.read();
083    // If it's really the startbyte (255) ...
084    if (startbyte == 255) {
085      // ... then get the next two bytes
086      for (i=0;i<2;i++) {
087        userInput[i] = Serial.read();
088      }
089      // First byte = servo to move?
090      servo = userInput[0];
091      // Second byte = which position?
092      pos = userInput[1];
093      // Packet error checking and recovery
094      if (pos == 255) { servo = 255; }
095 
096      // Assign new position to appropriate servo
097      switch (servo) {
098        case 1:
099             servo1.write(pos);//servo1.write(pos);
100          break;    
101        case 2:
102      servo2.write(pos);
103          break;
104        case 3:
105          servo3.write(pos);
106          break;
107        case 4:
108          servo4.write(pos);
109          break;
110 
111   // TO ADD SERVOS:
112   //     case 5:
113   //       servo5.write(pos);
114   //       break;
115   // etc...
116 
117        // LED on Pin 13 for digital on/off demo
118        case 99:
119          if (pos == 180) {
120            if (pinState == LOW) { pinState = HIGH; }
121            else { pinState = LOW; }
122          }
123          if (pos == 0) {
124            pinState = LOW;
125          }
126          digitalWrite(ledPin, pinState);
127          break;
128      }
129    }
130  }
131}