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