Add new control files
This commit is contained in:
Jacob Burroughs
parent
b42ee54a30
commit
7801c9c946
|
|
@ -0,0 +1,201 @@
|
|||
#include <EEPROM.h>
|
||||
#include <R2WD.h>
|
||||
|
||||
#include <fuzzy_table.h>
|
||||
#include <PID_Beta6.h>
|
||||
|
||||
#include <PinChangeInt.h>
|
||||
#include <PinChangeIntConfig.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -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");
|
||||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue