brian claus
tugboat project
Dependencies: TinyGPS HMC5883L MMA8451Q mbed PwmIn
Diff: main.cpp
- Revision:
- 7:e8a77af1b428
- Parent:
- 6:2d7e4561625c
--- a/main.cpp Thu Sep 05 18:55:20 2013 +0000
+++ b/main.cpp Thu Sep 12 13:57:36 2013 +0000
@@ -14,10 +14,10 @@
Timer t;
TinyGPS gps;
Serial gpsSerial(PTD3,PTD2);
-PwmIn rudderIn(PTD5);
-PwmIn thrusterIn(PTD0);
-PwmOut rudderOut(PTA4);
-PwmOut thrusterOut(PTA5);
+PwmIn thrusterIn(PTD5);
+PwmIn rudderIn(PTD0);
+PwmOut rudderOut(PTA5);
+PwmOut thrusterOut(PTA4);
void gpsSerialRecvInterrupt (void);
void radioSerialRecvInterrupt (void);
@@ -31,11 +31,11 @@
combo,
control};
-float rudderC = 0.0015, thrusterC = 0.0015;
+int rudderC = 0, thrusterC = 0;
int main(void) {
-
- int firstByte, secondByte;
+ float tLoop=0,tLoopi=0;
+ float rTime=0.00155,tTime=0.00165;
pc.baud(115200);
radio.baud(57600);
@@ -46,79 +46,67 @@
compass.init();
pc.printf("inited\r\n");
gpsSerial.attach (&gpsSerialRecvInterrupt, gpsSerial.RxIrq); // Recv interrupt handler
- //radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler
+ radio.attach (&radioSerialRecvInterrupt, radio.RxIrq); // Recv interrupt handler
gps.reset_ready();
rudderOut.period(0.02);
thrusterOut.period(0.02);
- rudderOut.pulsewidth(0.0015);
- thrusterOut.pulsewidth(0.0015);
+ rudderOut.pulsewidth(0.0016);
+ thrusterOut.pulsewidth(0.00165);
-
+ t.start();
while (true) {
-
+ tLoop =t.read();
if(gps.gga_ready()){
- pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
- radio.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
+ //pc.printf("lat: %.8f, lon: %.8f\r\n", gps.f_lat(), gps.f_lon());
+ radio.printf("GPS: %.4f, %.8f, %.8f, %.4f, %.4f\r\n", t.read(),gps.f_lat(), gps.f_lon(),gps.f_course(),gps.f_speed_knots());
}
acc.getAccAllAxis(dAcc);
- pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
- radio.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
+ //pc.printf("xA: %.4f, yA: %.4f, zA: %.4f\r\n", dAcc[0], dAcc[1], dAcc[2]);
+ radio.printf("ACC: %.4f, %.4f, %.4f, %.4f\r\n", t.read(),dAcc[0], dAcc[1], dAcc[2]);
compass.getXYZ(dCompass);
- pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
- radio.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
+ //pc.printf("xC: %4d, yC: %4d, zC: %4d\r\n", dCompass[0], dCompass[1], dCompass[2]);
+ radio.printf("MAG: %.4f, %4d, %4d, %4d\r\n", t.read(),dCompass[0], dCompass[1], dCompass[2]);
- if(radio.readable()){
- firstByte = radio.getc();
-
- if(firstByte == 'c'){
- manual = false;
- secondByte = radio.getc();
- switch(secondByte){
- case thruster:
- //example command 'ca 50'
- radio.scanf(" %f",thrusterC);
- break;
- case rudder:
- //example command 'cb -20'
- radio.scanf(" %f",rudderC);
- break;
- case combo:
- //example command 'cc -20,50'
- radio.scanf(" %f,%f",rudderC,thrusterC);
- break;
- case control:
- //example command 'cd'
- manual = true;
- break;
- default:
- //shouldn't be here flush stream
- radioFlush();
+ while(t.read() - tLoop < 0.5){
+ tLoopi=t.read();
+ if(manual){
+ //low pass filter the rudder time to stop the jitter
+ rTime=rTime + 0.8*(rudderIn.pulsewidth()-rTime);
+ //if outside limits center the rudder
+ if(rTime < 0.001 || rTime > 0.002)rTime=0.00155;
+ tTime =tTime +0.2*(thrusterIn.pulsewidth()-tTime);
+ //if outside limits thruster to zero
+ if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
+
+ rudderOut.pulsewidth(rTime);
+ thrusterOut.pulsewidth(tTime);
+ radio.printf("VEH:%.4f, %.6f, %.6f\r\n", t.read(),tTime, rTime);
}
- }
else{
- //we shouldnt be here do nothing
+ //rudderC should be an angle -45 to 45
+ //map this to 0.00135 to 0.00175
+ rTime=0.0014+0.003*((rudderC+45.0)/90.0);
+ //thrusterC should be an percentage from -100 to 100
+ //map this to 0.0011 to 0.0019
+ //0.0019 is full reverse
+ //0.0011 is full ahead
+ tTime=0.0019 - 0.008*((thrusterC+100.0)/200.0);
+ radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);
+ if(rTime < 0.001 || rTime > 0.002)rTime=0.00155;
+ if(tTime < 0.001 || tTime > 0.002)tTime=0.00165;
+ rudderOut.pulsewidth(rTime);
+
+ thrusterOut.pulsewidth(tTime);
+ radio.printf("VEH: %.4f, %.6f, %.6f, %.2f, %.2f\r\n",t.read(),rTime,tTime,rudderC,thrusterC);
}
- }
-
- if(manual){
- wait(0.5);
- rudderOut.pulsewidth(rudderIn.pulsewidth());
- thrusterOut.pulsewidth(thrusterIn.pulsewidth());
+ //make sure this doesn't run too fast
+ if((t.read()-tLoopi)<0.1){
+ wait(0.1-(t.read()-tLoopi));
}
- else{
- wait(0.5);
- //rudderC should be an angle -45 to 45
- //map this to 0.0013 to 0.0017
- if(rudderC > -45 && rudderC < 45){
- rudderOut.pulsewidth(0.0013+0.004*((rudderC+45)/90));
- }
- //thrusterC should be an percentage from 0 to 100
- //map this to 0.0013 to 0.0017
- thrusterOut.pulsewidth(0.0013 + 0.004*(thrusterC/100));
}
}
}
@@ -140,39 +128,55 @@
void radioSerialRecvInterrupt (void)
{
- int firstByte, secondByte;
- //get command
-
- firstByte = radio.getc();
- if(firstByte == 'c'){
- manual = false;
- secondByte = radio.getc();
- switch(secondByte){
- case thruster:
- //example command 'ca 50'
- radio.scanf(" %f",thrusterC);
- break;
- case rudder:
- //example command 'cb -20'
- radio.scanf(" %f",rudderC);
- break;
- case combo:
- //example command 'cc -20,50'
- radio.scanf(" %f,%f",rudderC,thrusterC);
- break;
- case control:
- //example command 'cd'
- manual = true;
- break;
- default:
- //shouldn't be here flush stream
- radioFlush();
+ int firstByte=0;
+ int command='a';
+
+ if(radio.readable())firstByte=radio.getc();
+
+ if(command=='t'){
+
+ thrusterC = firstByte-128;
+ //limit thrusterC
+ if(thrusterC>100){thrusterC=100;}
+ if(thrusterC<-100){thrusterC=-100;}
+ command=0;
+ }
+ else if(command=='r'){
+
+ rudderC=firstByte-128;
+ //limit rudderC
+ if(rudderC > 45){rudderC=45;}
+ if(rudderC<-45){rudderC=-45;}
+ command=0;
+ }
+
+ if(manual==true){
+ //get command
+ if(firstByte=='a'){
+ manual=false;
+ command='a';
+ }
+ }else{
+ //check if override
+ if(firstByte=='b'){
+ manual=true;
+ command='b';
+ }else if(firstByte=='r'){
+ //take in 0-255 as -45 to 45 where 128 is 0
+
+ command='r';
+
+ }else if(firstByte=='t'){
+
+ command=='t';
+ }else{
+ //do nothing, this will eat all the garbage chars...
}
- }
- else{
- //we shouldnt be here do nothing
- }
+
+ }
+
+
}