DTPA
/
Arduino
4
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Arduino/sketches/Nexus_Omni4WD/Nexus_Omni4WD.ino

349 lines
11 KiB
C++
Raw Blame History

/*
* Sonar sensors cannot be used when using the 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();
}
Reference in New Issue View Git Blame Copy Permalink