Diff: main.cpp

Revision:

3:e33676098294

Parent:

2:f72b9dd19159

--- a/main.cpp	Fri Mar 02 17:02:06 2018 +0000
+++ b/main.cpp	Tue Mar 27 13:22:52 2018 +0000
@@ -12,19 +12,109 @@
 Tach tLeft(p17,64);
 Tach tRight(p13,64);
 
-int main() {
-    //wait for MATLAB command    
+// speed controller
+Ticker spdCtrl;
+DigitalOut myled(LED1);
+
+// Function definition Prototypes (declarations)
+void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists
+
+
+float vl;
+float vr; 
+float Ts = 0.01;        // Control update period (seconds) (100 Hz equivalent)
+float speedL,speedR;
+float dcL;
+float dcR;
+float dcpL;
+float dcpR;
+float errpL;
+float errpR;
+float spdErrL;
+float spdErrR;
+float kp = 0.1;
+float ki = 0.01;
+float whl_rad = 3.1; // wheel radius in centimeters
+float baseL = 11.0; // wheelbase in centimeters
+float om = 0.0; // angular velocity
+float sp = 0.0; // forward speed in cm/s
+
+int main()
+{
+    //wait for MATLAB command
     left.speed(0.0);
     right.speed(0.0);
-    //while(1);
-    pc.getc();
-    //spin servos from -1 to 1 and print speed
-    for(float i=-1.0;i<=1.0;i+=0.01){
-        left.speed(-1.0*i);
-        right.speed(i);
-        wait_ms(100);
-        pc.printf("%f,%f\n", tLeft.getSpeed(),tRight.getSpeed());
+    while(1) {
+        spdErrL = 0.0;
+        spdErrR = 0.0;
+        dcL = 0.0;
+        dcR = 0.0;
+        errpL = 0.0;
+        dcpL = 0.0;
+        errpR = 0.0;
+        dcpR = 0.0;
+        pc.scanf("%f,%f,%f,%f",&sp,&om,&kp,&ki);
+
+        spdCtrl.attach(&ctrCode,Ts);
+
+        for(int i=1; i<=200; i++) {
+            
+            vl = (2*sp-om*baseL)/(2*whl_rad); // desired left wheel speed in cm/s
+            vr = (2*sp+om*baseL)/(2*whl_rad); // desired right wheel speed in cm/s
+            
+            
+            
+            wait_ms(100);
+            pc.printf("%f,%f,%f,%f\n", speedL,speedR,dcL,dcR);
+        }
+        
+        
+        
+        spdCtrl.detach();
+        left.stop();
+        right.stop();
+    }// end while(1)
+}
+
+
+// Additional function definitions
+void ctrCode() // function to attach to ticker
+{
+    myled = !myled; // toggle LED 2 to indicate control update
+
+    speedL = tLeft.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second
+    speedR = tRight.getSpeed()*2.0*3.14*whl_rad; // in centimeters per second
+
+    // compute error
+    spdErrL = vl-speedL;
+    spdErrR = vr-speedR;
+
+    // PI controller
+    dcL = dcpL+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrL+(Ts/2.0*(ki/kp)-1.0)*errpL); // compute duty cycle for motor using PI control scheme
+    dcR = dcpR+kp*((Ts/2.0*(ki/kp)+1.0)*spdErrR+(Ts/2.0*(ki/kp)-1.0)*errpR); // compute duty cycle for motor using PI control scheme
+
+    // enforce duty cycle saturation
+    if(dcL>1.0) {
+        dcL = 1.0;
+    } else if(dcL<0.0) {
+        dcL = 0.0;
     }
-    left.stop();
-    right.stop();
-}
+    if(dcR>1.0) {
+        dcR = 1.0;
+    } else if(dcR<0.0) {
+        dcR = 0.0;
+    }
+
+    // motor control
+    left.speed(dcL); // first input is the motor channel, second is duty cycle
+    right.speed(-dcR); // first input is the motor channel, second is duty cycle
+
+    // Age variables
+    errpL = spdErrL; // age speed error variable
+    dcpL = dcL; // age duty cycle variable
+
+    errpR = spdErrR; // age speed error variable
+    dcpR = dcR; // age duty cycle variable
+
+
+} // end ctrCode()