Diff: main.cpp

Revision:

13:d1f37c9038de

Parent:

6:1b4a677c468c

Child:

14:e55e80c1bd9a

--- a/main.cpp	Sat Apr 21 22:43:32 2018 +0000
+++ b/main.cpp	Sun Apr 22 21:47:24 2018 +0000
@@ -34,7 +34,6 @@
 AnalogIn speed(PTB0);   //tachometer
 AnalogIn _right(PTB1);  //Right sensor
 AnalogIn _left(PTB2);   //Left sensor
-//AnalogIn camIn(PTB3);   //Camera Input
 //Output
 PwmOut servoSig(PTA13); //PWM output to control servo position
 PwmOut gateDrive(PTA4); //PWM output to control motor speed
@@ -243,11 +242,15 @@
     if (L == 0.0 && R == 0.0)
     {
         //off track
-        //offTrack();
+        Setpoint = 0.0;
     }           
 //    float fb = L - R; 
-    float fb = ((float)camMax)/64.0;;
+    float fb = (float)camMax/64;
     float e = SET - fb;
+    Kps = 0.005/speed.read();
+    if (Kps > 0.02) Kps = 0.02;
+    Kd = 1.0e-3/speed.read();
+    if (Kd > 1.0e-4) Kd = 1.0e-4;
     float Controlleroutput = Kps * e - (Kd * (fb - fbPrev)/TI)+ BIAS;//subtract derivative of error?? 
     if (Controlleroutput > MAXS) Controlleroutput = MAXS;
     else if (Controlleroutput < MINS) Controlleroutput = MINS;
@@ -257,10 +260,10 @@
         servoSig.write(Controlleroutput);
     }
     fbPrev = fb;
-    data[0] = _right.read();
-    data[1] = _left.read();
-    data[2] = Controlleroutput;
-    data[3] = e;
+//    data[0] = _right.read();
+//    data[1] = _left.read();
+//    data[2] = Controlleroutput;
+//    data[3] = e;
     
 }
 void drive()
@@ -275,8 +278,8 @@
     if(output < MINM) output = MINM;        
 
     gateDrive.write(output);
-    data[4] = gateDrive.read();
-    data[5] = speed.read(); 
+//    data[4] = gateDrive.read();
+//    data[5] = speed.read(); 
 }
 
 void cb()
@@ -314,15 +317,15 @@
     waitState();
     while(1) {
         
-        if(lTrig){
-            bt.putc('l');
-            lTrig = false;
-        }
-        if(rTrig){
-            bt.putc('r');
-            rTrig = false;
-        }
-        //bt.printf("Cammax %i\r\n", camMax);
+//        if(lTrig){
+//            bt.putc('l');
+//            lTrig = false;
+//        }
+//        if(rTrig){
+//            bt.putc('r');
+//            rTrig = false;
+//        }
+        bt.printf("%i\r\n", camMax);
 
     }
 }
\ No newline at end of file