Revision 2:65e922b3569d, committed 2021-09-15

Comitter:

thomasgg

Date:

Wed Sep 15 16:07:21 2021 +0000

Parent:

1:ac92cf476127

Commit message:

Committed to current version

Changed in this revision

--- a/main.cpp	Thu May 13 20:44:33 2021 +0000
+++ b/main.cpp	Wed Sep 15 16:07:21 2021 +0000
@@ -3,6 +3,8 @@
 #include <ros.h>
 #include <std_msgs/String.h>
 #include <std_msgs/Float32.h>
+
+#include "millis.h"
  
 // ===== ENCODERS
 // motor_pulses_per_rev
@@ -11,6 +13,32 @@
 // Gear ratio / (2 * pi) * encoder_counts
 float pulses_per_wheel_rad = (35 / 28) * (2000) / (2 * 3.14159);
 
+// Prev encoder value
+float wheel0 = 0;
+float wheel1 = 0;
+float wheel2 = 0;
+float wheel3 = 0;
+
+float wheel0_prev = 0;
+float wheel1_prev = 0;
+float wheel2_prev = 0;
+float wheel3_prev = 0;
+
+float kP = 0.05;
+float kF = 0.035;
+
+int pub_skip = 2;
+int count = 0;
+
+float dt = 0.005;
+float prev_t = 0;
+
+// Motor speed set points
+float motor0_sp = 0;
+float motor1_sp = 0;
+float motor2_sp = 0;
+float motor3_sp = 0;
+
 // Verify encoder lengths
 QEI encP0(A0, A2, NC, pulses_per_rev);
 QEI encP1(D6, D12, NC, pulses_per_rev);
@@ -37,9 +65,7 @@
 PwmOut motor0_En(D11);
 
 
-// ======= MOTOR CALLBACKS
-void motor0_callback(const std_msgs::Float32& msg){
-    float power = msg.data;
+void setMotor0Power(float power){
     if( power > 0 ){
         motor0A = 1;
         motor0B = 0;
@@ -53,8 +79,7 @@
     motor0_En = abs(power);
 }
 
-void motor1_callback(const std_msgs::Float32& msg){
-    float power = msg.data;
+void setMotor1Power(float power){
     if( power > 0 ){
         motor1A = 1;
         motor1B = 0;
@@ -68,12 +93,11 @@
     motor1_En = abs(power);
 }
 
-void motor2_callback(const std_msgs::Float32& msg){
-    float power = msg.data;
+void setMotor2Power(float power){
     if( power > 0 ){
         motor2A = 1;
         motor2B = 0;
-    }else if( power < 0 ){
+    }else if(power < 0 ){
         motor2A = 0;
         motor2B = 1;
     } else {
@@ -83,8 +107,7 @@
     motor2_En = abs(power);
 }
 
-void motor3_callback(const std_msgs::Float32& msg){
-    float power = msg.data;
+void setMotor3Power(float power){
     if( power > 0 ){
         motor3A = 1;
         motor3B = 0;
@@ -98,6 +121,22 @@
     motor3_En = abs(power);
 }
 
+// ======= MOTOR CALLBACKS
+void motor0_callback(const std_msgs::Float32& msg){
+    motor0_sp = msg.data;
+}
+
+void motor1_callback(const std_msgs::Float32& msg){
+    motor1_sp = msg.data;
+}
+
+void motor2_callback(const std_msgs::Float32& msg){
+    motor2_sp = msg.data;
+}
+
+void motor3_callback(const std_msgs::Float32& msg){
+    motor3_sp = msg.data;
+}
 
     
 
@@ -143,32 +182,49 @@
     nh.subscribe(motor3);
     
     while (1) {
-        //led = !led; // Toggle LED
         
-        // ENCODER P4
-        enc_pos0.data = (float) encP0.getPulses() / pulses_per_wheel_rad;
-        //enc_pos0.data = 10;
-        encoderP0.publish( &enc_pos0 );
+        //dt = (millis() / 1000.0) - prev_t;
+        //prev_t = (millis() / 1000.0);
         
-        // ENCODER P3
-        enc_pos1.data = (float) encP1.getPulses() / pulses_per_wheel_rad;
-        //enc_pos1.data = 10;
-        encoderP1.publish( &enc_pos1 );
+        if(count >= pub_skip){
+            
+            // ENCODER P4
+            wheel0 = (float) encP0.getPulses() / pulses_per_wheel_rad;
+            setMotor0Power( -kP * ((wheel0 - wheel0_prev)/dt - motor0_sp) + kF * motor0_sp);
+            enc_pos0.data = wheel0;
+            encoderP0.publish( &enc_pos0 );
+            wheel0_prev = wheel0;
+            
+            // ENCODER P3
+            wheel1 = (float) encP1.getPulses() / pulses_per_wheel_rad;
+            setMotor1Power( -kP * ((wheel1 - wheel1_prev)/dt - motor1_sp) + kF * motor1_sp);
+            enc_pos1.data = wheel1;
+            encoderP1.publish( &enc_pos1 );
+            wheel1_prev = wheel1;
+            
+            // ENCODER P2
+            wheel2 = (float) encP2.getPulses() / pulses_per_wheel_rad;
+            setMotor2Power( -kP * ((wheel2 - wheel2_prev)/dt - motor2_sp) + kF * motor2_sp);
+            enc_pos2.data = wheel2;
+            encoderP2.publish( &enc_pos2 );
+            wheel2_prev = wheel2;
+            
+            // ENCODER P3
+            wheel3 = -(float) encP3.getPulses() / pulses_per_wheel_rad;
+            setMotor3Power( -kP * ((wheel3 - wheel3_prev)/dt - motor3_sp) + kF * motor3_sp);
+            enc_pos3.data = wheel3;
+            encoderP3.publish( &enc_pos3 );
+            wheel3_prev = wheel3;
+            
+            count = 0;
         
-        // ENCODER P2
-        enc_pos2.data = (float) encP2.getPulses() / pulses_per_wheel_rad;
-        //enc_pos2.data = 10;
-        encoderP2.publish( &enc_pos2 );
-        
-        // ENCODER P3
-        enc_pos3.data = -(float) encP3.getPulses() / pulses_per_wheel_rad;
-        //enc_pos3.data = 10;
-        encoderP3.publish( &enc_pos3 );
+        }
         
         
         // Spin the rosserial node handler
         nh.spinOnce();
         
-        wait(0.01); // wait.....
+        wait(dt); // wait.....
+        count ++;
     }
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/millis.lib	Wed Sep 15 16:07:21 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/hudakz/code/millis/#ac7586424119