diff --git a/car_movement/car_movement.ino b/car_movement/car_movement.ino new file mode 100644 index 0000000..8e3ccec --- /dev/null +++ b/car_movement/car_movement.ino @@ -0,0 +1,201 @@ +#include +#include + +#include +#include + +#include +#include +#include + +#include "commands.h" +/*********************************************/ + +irqISR(irq1, isr1); +MotorWheel wheel1(3, 2, 4, 5, &irq1, REDUCTION_RATIO, int(144 * PI)); + +irqISR(irq2, isr2); +MotorWheel wheel2(11, 12, 6, 7, &irq2, REDUCTION_RATIO, int(144 * PI)); + +//R2WD _2WD(&wheel1, &wheel2, WHEELSPAN); + +//bool myLED = HIGH; +String inputString = ""; +String prevString = ""; +boolean stringComplete = false; +char buffer[100]; + +void setup() { + Serial.begin(9600); + // reserve 200 bytes for the inputString: + inputString.reserve(20); + prevString.reserve(20); + //TCCR0B=TCCR0B&0xf8|0x01; // warning!! it will change millis() + TCCR1B = TCCR1B & 0xf8 | 0x01; // Pin9,Pin10 PWM 31250Hz + TCCR2B = TCCR2B & 0xf8 | 0x01; // Pin3,Pin11 PWM 31250Hz + wheel1.PIDEnable(0.35, 0.02, 0, 10); + wheel2.PIDEnable(0.35, 0.02, 0, 10); + //_2WD.PIDRegulate(); +} + +int targetLeftDist = -1; +int targetRightDist = -1; + +int targetLeftSpeed = 0; +int targetRightSpeed = 0; + +int activeLeftSpeed = 0; +int activeRightSpeed = 0; + +void resetPosition() { + wheel1.resetCurrPulse(); + wheel2.resetCurrPulse(); +} + +void checkPosition() { + if(targetLeftDist >= 0 && abs(wheel1.getCurrPulse()) >= targetLeftDist) { + targetLeftDist = -1; + targetLeftSpeed = 0; + activeLeftSpeed = 0; + Serial.println("done,l;"); + } + if(targetRightDist >= 0 && abs(wheel2.getCurrPulse()) >= targetRightDist) { + targetRightDist = -1; + targetRightSpeed = 0; + activeRightSpeed = 0; + Serial.println("done,r;"); + } +} + +void setDistances(int leftDist, int rightDist) { + targetLeftDist = leftDist; + targetRightDist = rightDist; +} + +void setTargetSpeeds(int leftSpeed, int rightSpeed) { + targetLeftSpeed = leftSpeed; + targetRightSpeed = rightSpeed; + setSpeeds(); +} + +void updateSpeeds() { + if(targetLeftSpeed < activeLeftSpeed) activeLeftSpeed--; + if(targetLeftSpeed > activeLeftSpeed) activeLeftSpeed++; + if(targetRightSpeed < activeRightSpeed) activeRightSpeed--; + if(targetRightSpeed > activeRightSpeed) activeRightSpeed++; +} + +void setSpeeds() { + int rightSpeed = -activeRightSpeed; + int leftSpeed = activeLeftSpeed; + bool leftDirection = DIR_ADVANCE; + bool rightDirection = DIR_ADVANCE; + if(leftSpeed < 0) { + leftSpeed = -leftSpeed; + leftDirection = DIR_BACKOFF; + } + if(rightSpeed < 0) { + rightSpeed = -rightSpeed; + rightDirection = DIR_BACKOFF; + } + wheel1.setSpeedMMPS(leftSpeed, leftDirection); + wheel2.setSpeedMMPS(rightSpeed, rightDirection); +} + +void loop() { + if (stringComplete) { + inputString.trim(); +// Serial.println("Your input: " + inputString); + if (inputString.equals("test;")) { +// _2WD.setCarBackoffDistance(60, 1400); +// _2WD.setCarRotateLeftAngle(100, PI / 8 ); +// _2WD.setCarRotateRight(0); +// _2WD.setCarSpeedMMPS(100, 1000); +// _2WD.setCarAdvance(0); +// _2WD.setCarSpeedMMPS(60, 2000); +//// _2WD.delayMS(4000); +//// _2WD.setCarSlow2Stop(500); + } + if (inputString.startsWith(MOVE_FORWARD)) { + int speed = inputString.substring(MOVE_FORWARD.length()).toInt(); + int distance = inputString.substring(inputString.lastIndexOf(',')+1,inputString.lastIndexOf(';')).toInt(); + distance *= 80; + setDistances(distance, distance); + setTargetSpeeds(speed, speed); + resetPosition(); + } else if (inputString.startsWith(MOVE_BACKWARD)) { + int speed = inputString.substring(MOVE_BACKWARD.length()).toInt(); + int distance = inputString.substring(inputString.lastIndexOf(',')+1,inputString.lastIndexOf(';')).toInt(); + distance *= 80; + setDistances(distance, distance); + setTargetSpeeds(-speed, -speed); + resetPosition(); + } else if (inputString.startsWith(STOP)) { + int speed = inputString.substring(STOP.length()).toInt(); + activeLeftSpeed = 0; + targetLeftSpeed = 0; + activeRightSpeed = 0; + targetRightSpeed = 0; + resetPosition(); + } else if (inputString.startsWith(TURN_RIGHT)) { + int speed = inputString.substring(TURN_RIGHT.length()).toInt(); + int angle = inputString.substring(inputString.lastIndexOf(',')+1,inputString.lastIndexOf(';')).toInt(); + angle *= 5.9; + setDistances(angle, angle); + setTargetSpeeds(-speed, speed); + resetPosition(); + } else if (inputString.startsWith(TURN_LEFT)) { + int speed = inputString.substring(TURN_LEFT.length()).toInt(); + int angle = inputString.substring(inputString.lastIndexOf(',')+1,inputString.lastIndexOf(';')).toInt(); + angle *= 5.9; + setDistances(angle, angle); + setTargetSpeeds(speed, -speed); + resetPosition(); + } else if (inputString.startsWith(TURN)) { + int leftSpeed = inputString.substring(TURN.length()).toInt(); + int rightSpeed = inputString.substring(inputString.indexOf(',')+1,inputString.lastIndexOf(',')).toInt(); + int distance = inputString.substring(inputString.lastIndexOf(',')+1,inputString.lastIndexOf(';')).toInt(); + distance *= 80; + setDistances(-1, -1); + setTargetSpeeds(leftSpeed, rightSpeed); + resetPosition(); + } else if (inputString.startsWith(STATUS)) { + Serial.println("Current actual left speed: " + String(activeLeftSpeed)); + Serial.println("Current target left speed: " + String(targetLeftSpeed)); + Serial.println("Current actual right speed: " + String(activeRightSpeed)); + Serial.println("Current target right speed: " + String(targetRightSpeed)); + } + prevString = inputString; + // clear the string: + inputString = ""; + stringComplete = false; + } +// Serial.println("Current actual left speed: " + String(activeLeftSpeed)); +// Serial.println("Current target left speed: " + String(targetLeftSpeed)); + updateSpeeds(); + setSpeeds(); + checkPosition(); + wheel1.PIDRegulate(); + wheel2.PIDRegulate(); +} + +/* + SerialEvent occurs whenever a new data comes in the + hardware serial RX. This routine is run between each + time loop() runs, so using delay inside loop can delay + response. Multiple bytes of data may be available. +*/ +void serialEvent() { + while (Serial.available()) { + // get the new byte: + char inChar = (char)Serial.read(); + // add it to the inputString: + inputString += inChar; + // if the incoming character is a newline, set a flag + // so the main loop can do something about it: + if (inChar == ';') { + stringComplete = true; + } + } +} + diff --git a/car_movement/commands.h b/car_movement/commands.h new file mode 100644 index 0000000..b522c4c --- /dev/null +++ b/car_movement/commands.h @@ -0,0 +1,7 @@ +const String MOVE_FORWARD = String('g'); +const String MOVE_BACKWARD = String('b'); +const String STOP = String('s'); +const String TURN_RIGHT = String("rl"); +const String TURN_LEFT = String("rr"); +const String TURN = String("r"); +const String STATUS = String("h"); diff --git a/nexus.py b/nexus.py new file mode 100644 index 0000000..522407a --- /dev/null +++ b/nexus.py @@ -0,0 +1,54 @@ +import serial +import time + +class NexusRobot: + + def __init__(self, port): + self.ser = serial.serial_for_url(port, 9600, do_not_open = True) + self.ser.open() + time.sleep(3) + + def _wait(self): + self.ser.readline() + self.ser.readline() + + def pause(self): + time.sleep(1) + + def forward(self, speed, dist): + self.ser.write("g%d,%d;" % (speed, dist)) + self._wait() + + def reverse(self, speed, dist): + self.ser.write("b%d,%d;" % (speed, dist)) + self._wait() + + def turnRight(self, speed, angle): + self.ser.write("rr%d,%d;" % (speed, angle)) + self._wait() + + def turnLeft(self, speed, angle): + self.ser.write("rl%d,%d;" % (speed, angle)) + self._wait() + + + + + +robot = NexusRobot('/dev/cu.usbserial-AL00YYCA') +robot.forward(200,10) +robot.pause() +robot.turnRight(200,90) +robot.pause() +robot.forward(200,10) +robot.pause() +robot.turnRight(200,90) +robot.pause() +robot.forward(200,10) +robot.pause() +robot.turnRight(200,90) +robot.pause() +robot.forward(200,10) +robot.pause() +robot.turnRight(200,90) +robot.pause() \ No newline at end of file