Diff: main.cpp

Revision:

47:31726ce58a6d

Parent:

46:e5fb4dd7b531

Child:

48:72bba06c3680

--- a/main.cpp	Wed May 18 09:29:01 2022 -0500
+++ b/main.cpp	Thu May 19 02:23:27 2022 -0500
@@ -31,11 +31,28 @@
     DigitalOut enable_motors(PB_15);
     FastPWM pwm_M1(PB_13);
     FastPWM pwm_M2(PA_9);
+    EncoderCounter  encoder_M1(PA_6, PC_7);  //M1 to M-, M2 to M+
+    EncoderCounter  encoder_M2(PB_6, PB_7); 
     
-    const float pwm_period_s = .00005f;
-    pwm_M1.period(pwm_period_s);
-    pwm_M2.period(pwm_period_s);
-    //pwm_M1.write(0.5f);
+    //Open loop motor control
+    //const float pwm_period_s = .00005f;
+    //pwm_M1.period(pwm_period_s);
+    //pwm_M2.period(pwm_period_s);
+
+    //Speed controller
+    const float max_voltage = 12.0f;
+    const float counts_per_turn = 20.0f * 78.125; //encoder counts * gear ratio
+    const float kn = 180.0f/12.0f; // motor constant on pololu website
+
+    SpeedController speedControllerM1(counts_per_turn, kn, max_voltage, pwm_M1, encoder_M1);
+    SpeedController speedControllerM2(counts_per_turn, kn, max_voltage, pwm_M2, encoder_M2);
+
+    speedControllerM1.setSpeedCntrlGain(0.1f/3);
+    speedControllerM2.setSpeedCntrlGain(0.1f/3);
+
+    speedControllerM1.setMaxAccelerationRPM(999999.0f);
+    speedControllerM2.setMaxAccelerationRPM(999999.0f);
+
 
     enable_motors = 1;
 
@@ -65,9 +82,15 @@
 
         //pwm_M1.write(.75f);
         if (do_execute_main_task) {
-            pwm_M1.write(.75f);
+            //pwm_M1.write(.75f);
+            //pwm_M2.write(.75f);
+            speedControllerM1.setDesiredSpeedRPS(1.5f); // max 3
+            speedControllerM2.setDesiredSpeedRPS(1.5f);
         } else {
-            pwm_M1.write(.5f);
+            //pwm_M1.write(.5f);
+            //pwm_M2.write(.5f);
+            speedControllerM1.setDesiredSpeedRPS(0.0f);
+            speedControllerM2.setDesiredSpeedRPS(0.0f);
         } 
         
 
@@ -80,7 +103,8 @@
         //printf("IR sensor (mV): %f \r\n", ir_distance_mV);
         //printf("IR sensor (cm): %f \r\n", ir_distance_cm);
 
-        
+
+        printf("%f \r\n", speedControllerM2.getSpeedRPS());   
         
         
         // do only output via serial what's really necessary (this makes your code slow)