Diff: src/main.cpp

Revision:

6:ed47deb76adf

Parent:

5:44b2454a5eea

--- a/src/main.cpp	Tue Jun 08 15:04:33 2021 +0000
+++ b/src/main.cpp	Tue Jun 08 20:55:08 2021 +0000
@@ -1,4 +1,6 @@
 /** @file main.cpp
+ * @author Simon Krogedal
+ * @version 0.1
  */
 
 /* Karbot motor controller
@@ -32,10 +34,18 @@
 #define     R_MOTOR_DIR     PB_10
 #define     L_MOTOR_DIR     PA_9
 
-#define     R_ENC_PIN_A     PB_5
-#define     R_ENC_PIN_B     PB_4
+#define     R_ENC_PIN_A     PB_5    // Blue
+#define     R_ENC_PIN_B     PB_4    // Purple
 #define     L_ENC_PIN_A     PB_3
 #define     L_ENC_PIN_B     PC_13
+
+/* These pin definitions were used in ESP, it might be smart to go use those
+#define     L_ENC_PIN_A     PA_14
+#define     L_ENC_PIN_B     PB_15
+#define     R_ENC_PIN_A     PA_13
+#define     R_ENC_PIN_B     PB_1
+*/
+
 #define     BAT_A_PIN       PA_0
 #define     BAT_B_PIN       PA_1
 
@@ -45,7 +55,7 @@
 
 
 //VARIABLES
-#define     MOTOR_FREQ      25000.0         // 25 kHz, freq of motor PWM
+#define     MOTOR_FREQ      20000.0         // 20 kHz, freq of motor PWM, max for driver board
 #define     MOTOR_TS        0.05            // Sample time, ie the time between every PID calculation
 #define     MOTOR_KP        0.00002         // Loop motor gain
 #define     MOTOR_TI        200             // Integral motor gain
@@ -75,8 +85,8 @@
 motor           leftMot     (L_MOTOR_PIN, L_MOTOR_DIR, 1/MOTOR_FREQ);
 motor           rightMot    (R_MOTOR_PIN, R_MOTOR_DIR, 1/MOTOR_FREQ);
 
-MotorControl    leftCtrl    (L_ENC_PIN_A, L_ENC_PIN_B, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD);
-MotorControl    rightCtrl   (R_ENC_PIN_A, R_ENC_PIN_B, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD);
+MotorControl    leftCtrl    (L_ENC_PIN_A, L_ENC_PIN_B, ENCODER_CPR, encoder::Left, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD);
+MotorControl    rightCtrl   (R_ENC_PIN_A, R_ENC_PIN_B, ENCODER_CPR, encoder::Right, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, WHEEL_RAD);
 
 // Callback function for a twist message given by the motion controller
 void messageCb(const geometry_msgs::Twist& msg);
@@ -107,9 +117,12 @@
 
     
     // Set up joint state message
-    //wheels_msg.name = ["Left wheel","Right wheel"];
-    wheels_msg.name[0] = "Left wheel";
-    wheels_msg.name[1] = "Right wheel";
+    wheels_msg.name_length = 2;             // In these libs, lenght must be set
+    wheels_msg.velocity_length = 2;         // manually for each term
+    
+    // Name joints
+    char *jointNames[] = {"Left wheel","Right wheel"};
+    wheels_msg.name = jointNames;
     
     // Set up battery messages
     batA_msg.power_supply_technology    =   2;
@@ -180,8 +193,8 @@
         nh.advertise(right_wheel_pub);
         nh.loginfo("Setup complete");
         
-        leftCtrl.start();
-        rightCtrl.start();
+        //leftCtrl.start();
+        //rightCtrl.start();
         
         // Main loop
         while(true) {
@@ -209,8 +222,8 @@
             batB_pub.publish(&batB_msg);
             wheels_pub.publish(&wheels_msg);
             
-            left_wheel_pub.publish(@left_wheel_speed_msg);
-            right_wheel_pub.publish(@right_wheel_speed_msg);
+            left_wheel_pub.publish(&left_wheel_speed_msg);
+            right_wheel_pub.publish(&right_wheel_speed_msg);
             
             nh.spinOnce();