349 lines
11 KiB
C++
349 lines
11 KiB
C++
/*
|
|
* Sensors cannot be used together with serial interface!
|
|
* Build for Duemilanove with 328.
|
|
*
|
|
* Changes to version 30-10-2018:
|
|
* - The Sonar code has been completely stripped out because the serial port is not available for the Sonars.
|
|
* - Brackets has been placed in the two code lines for setting the PWM frequency, because it was causing compiler warnings.
|
|
* - The function allStop has been modified according the solution provided by asantiago@bnl.gov
|
|
*/
|
|
|
|
template<class T> inline Print &operator <<(Print &obj, T arg) { obj.print(arg); return obj; }
|
|
|
|
#include <MotorWheel.h>
|
|
#include <Omni4WD.h>
|
|
#include <PID_Beta6.h>
|
|
#include <PinChangeInt.h>
|
|
#include <PinChangeIntConfig.h>
|
|
//#include <SONAR.h>
|
|
|
|
/*
|
|
************************************************************************************
|
|
Sonar:0x12
|
|
------------------------------
|
|
| |
|
|
M3| |M2
|
|
| |
|
|
| |
|
|
| |
|
|
Sonar:0x13| |Sonar:0x11
|
|
| |
|
|
| |Power Switch
|
|
| |
|
|
| |
|
|
-------------------------------
|
|
| |
|
|
M4| |M1
|
|
| |
|
|
-------------------------------
|
|
Sonar:0x14
|
|
************************************************************************************
|
|
*/
|
|
|
|
|
|
int speedRampTime = 1000; //speed ramping time in ms.
|
|
|
|
irqISR(irq1,isr1);
|
|
MotorWheel wheel1(3,2,4,5,&irq1);
|
|
|
|
irqISR(irq2,isr2);
|
|
MotorWheel wheel2(11,12,14,15,&irq2);
|
|
|
|
irqISR(irq3,isr3);
|
|
MotorWheel wheel3(9,8,16,17,&irq3);
|
|
|
|
irqISR(irq4,isr4);
|
|
MotorWheel wheel4(10,7,18,19,&irq4);
|
|
|
|
Omni4WD Omni(&wheel1,&wheel2,&wheel3,&wheel4);
|
|
|
|
|
|
|
|
// er zitten delays in deze functies.
|
|
void goAhead(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_ADVANCE) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarAdvance(0);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void turnLeft(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_LEFT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarLeft(0);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void turnRight(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_RIGHT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarRight(0);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void rotateRight(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_ROTATERIGHT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarRotateRight(0);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void rotateLeft(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_ROTATELEFT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarRotateLeft(0);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void allStop(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_STOP) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarStop();
|
|
//this sets the PWM pins Low to enforce a stop. Solution from asantiago@bnl.gov via email conversation.
|
|
digitalWrite(3, LOW);digitalWrite(11, LOW);
|
|
digitalWrite(9, LOW);digitalWrite(10, LOW);
|
|
}
|
|
|
|
void backOff(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_BACKOFF) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarBackoff(0);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void upperLeft(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_UPPERLEFT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarUpperLeft(speedMMPS);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void lowerLeft(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_LOWERLEFT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarLowerLeft(speedMMPS);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void upperRight(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_UPPERRIGHT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarUpperRight(speedMMPS);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
void lowerRight(int speedMMPS){
|
|
if(Omni.getCarStat()!=Omni4WD::STAT_LOWERRIGHT) Omni.setCarSlow2Stop(speedRampTime);
|
|
Omni.setCarLowerRight(speedMMPS);
|
|
Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
|
|
}
|
|
|
|
/*
|
|
backoff = achteruit
|
|
turnLeft = opzij links
|
|
upperLeft = diagonaal links naar voren
|
|
*/
|
|
|
|
void(*function[11])(int speedMMPS) =
|
|
{goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop,
|
|
backOff, upperLeft, lowerLeft, upperRight, lowerRight};
|
|
|
|
|
|
#define NULLCHAR 0
|
|
#define SPACE 32
|
|
#define ESC 27
|
|
#define BACKSPACE 8
|
|
#define TAB 9
|
|
#define LF 10
|
|
#define CR 13
|
|
|
|
// Last active time
|
|
unsigned long int lastactime, currMillis, debtime;
|
|
|
|
char autoshutdown = 0;
|
|
char autoreply = 0;
|
|
char repeatmode = 0;
|
|
|
|
char sendpacket[24];
|
|
char error_code = 0;
|
|
|
|
int speedMMPS = 0;
|
|
|
|
void setup() {
|
|
delay(2000);
|
|
TCCR1B = (TCCR1B & 0xf8) | 0x01; // Pin9,Pin10 PWM 31250Hz
|
|
TCCR2B = (TCCR2B & 0xf8) | 0x01; // Pin3,Pin11 PWM 31250Hz
|
|
|
|
//SONAR::init(13);
|
|
//Omni.switchMotors();
|
|
Omni.PIDEnable(0.35,0.02,0,10);
|
|
|
|
Serial.begin(28800); // opens serial port, sets data rate to 28800 bps.
|
|
// Faster does not work with all motor running fast.
|
|
lastactime = currMillis = millis();
|
|
|
|
sendpacket[0] = 2; // STX
|
|
sendpacket[23] = 3;// ETX
|
|
}
|
|
|
|
void sendinfo(void) {
|
|
int i;
|
|
|
|
sendpacket[1] = 1; // info packet
|
|
sendpacket[2] = 1; // version
|
|
sendpacket[3] = 1;
|
|
sendpacket[4] = error_code;
|
|
sendpacket[5] = Omni.getCarStat();
|
|
sendpacket[6] = (speedMMPS ) & 0xff ;
|
|
sendpacket[7] = (speedMMPS >> 8) & 0xff;
|
|
|
|
Serial.write(sendpacket, 24);
|
|
|
|
/* getCarStat --> info
|
|
Serial.print("Info: ");
|
|
Serial.print(" ");
|
|
Serial.print((char)Omni.getCarStat());
|
|
Serial.print(" ");
|
|
Serial.print(distBuf[0]);
|
|
Serial.print(" ");
|
|
Serial.print(distBuf[0]);
|
|
Serial.print(" ");
|
|
Serial.print(distBuf[0]);
|
|
Serial.print(" ");
|
|
Serial.print(distBuf[0]);
|
|
Serial.println();
|
|
*/
|
|
}
|
|
|
|
|
|
|
|
// number of received chars from next packet
|
|
unsigned char nmbr = 0;
|
|
unsigned char packet[16];
|
|
char dosendinfo = 0;
|
|
|
|
void loop() {
|
|
char c;
|
|
//unsigned char sonarcurrent = 0;
|
|
short frontleft, backleft, frontright, backright;
|
|
|
|
// try to complete getting a packet and then process it.
|
|
if (Serial.available() > 0) {
|
|
c = Serial.read();
|
|
packet[nmbr++] = c;
|
|
if (nmbr > 15) { // process next packet
|
|
if (packet[0] != 2 || packet[15] != 3) {
|
|
// set error in info packet
|
|
error_code = 1;
|
|
//Serial.println("Packet framing error");
|
|
}
|
|
/* Serial.println((int)packet[0]);
|
|
Serial.println((int)packet[1]);
|
|
Serial.println((int)packet[2]);
|
|
Serial.println((int)packet[3]);
|
|
Serial.println((int)packet[4]);
|
|
Serial.println((int)packet[5]);
|
|
Serial.println((int)packet[6]);
|
|
Serial.println((int)packet[7]);
|
|
Serial.println();
|
|
|
|
Serial.println("Process Command");
|
|
*/
|
|
if (packet[1] < 11) {
|
|
speedMMPS = (int)((unsigned int)packet[2]+((unsigned int)packet[3])*256);
|
|
(*function[packet[1]])(speedMMPS);
|
|
//Serial.println(packet[1]);
|
|
//Serial.println(speedMMPS);
|
|
}
|
|
else {
|
|
switch (packet[1]) {
|
|
case 12: // set speed of all four motors
|
|
//debtime = millis();
|
|
frontleft = (short)((unsigned short)packet[2]+((unsigned short)packet[3])*256);
|
|
backleft = (short)((unsigned short)packet[4]+((unsigned short)packet[5])*256);
|
|
frontright = (short)((unsigned short)packet[6]+((unsigned short)packet[7])*256);
|
|
backright = (short)((unsigned short)packet[8]+((unsigned short)packet[9])*256);
|
|
if (frontleft > 700) frontleft = 700;
|
|
else if (frontleft < -700) frontleft = -700;
|
|
if (frontright > 700) frontright = 700;
|
|
else if (frontright < -700) frontright = -700;
|
|
if (backleft > 700) backleft = 700;
|
|
else if (backleft < -700) backleft = -700;
|
|
if (backright > 700) backright = 700;
|
|
else if (backright < -700) backright = -700;
|
|
|
|
if ( (frontleft==0) && (backleft==0) ) {
|
|
allStop(0);
|
|
}
|
|
else {
|
|
|
|
Omni.wheelULSetSpeedMMPS(frontleft);
|
|
Omni.wheelLLSetSpeedMMPS(backleft);
|
|
Omni.wheelLRSetSpeedMMPS(-frontright);
|
|
Omni.wheelURSetSpeedMMPS(-backright);
|
|
}
|
|
|
|
/*
|
|
debtime = millis()- debtime;
|
|
debtime = (short int) debtime;
|
|
sendpacket[10] = (debtime ) & 0xff ;
|
|
sendpacket[11] = (debtime >> 8) & 0xff;
|
|
sendinfo();
|
|
*/
|
|
break;
|
|
case 13: // set speedMMPS
|
|
speedMMPS = (int)((unsigned short)packet[2]+((unsigned short)packet[3])*256);
|
|
if (speedMMPS > 700) speedMMPS = 700;
|
|
else if (speedMMPS < -700) speedMMPS = -700;
|
|
break;
|
|
case 20: // ask info packet
|
|
dosendinfo = 1;
|
|
break;
|
|
case 21:
|
|
autoshutdown = 0;
|
|
break;
|
|
case 22:
|
|
autoshutdown = 1;
|
|
break;
|
|
case 23:
|
|
autoreply = 0;
|
|
break;
|
|
case 24:
|
|
autoreply = 1;
|
|
break;
|
|
case 25:
|
|
error_code = 2;
|
|
//Serial.println("25 Not implemented");
|
|
break;
|
|
case 26:
|
|
error_code = 3;
|
|
//Serial.println("Not implemented");
|
|
break;
|
|
case 27: // keep alive
|
|
break;
|
|
case 28:
|
|
repeatmode = 0;
|
|
break;
|
|
case 29:
|
|
repeatmode = 1;
|
|
break;
|
|
default:
|
|
error_code = 3;
|
|
//Serial.println("Unrecognized command");
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (autoreply || dosendinfo) sendinfo();
|
|
dosendinfo = 0;
|
|
|
|
// clear packet buffer
|
|
nmbr = 0;
|
|
lastactime = millis();
|
|
}
|
|
}
|
|
|
|
|
|
if (autoshutdown) {
|
|
if ((millis() - lastactime) > 3000) {
|
|
//Serial.print("Connection lost?");
|
|
//Serial.print(millis());
|
|
//Serial.println();
|
|
allStop(0);
|
|
lastactime = millis();
|
|
}
|
|
}
|
|
Omni.PIDRegulate();
|
|
}
|
|
|