Diff: src/main.cpp

Revision:

3:4b6080e86761

Parent:

2:aeaa1c1e0d33

Child:

5:44b2454a5eea

--- a/src/main.cpp	Thu May 27 19:26:21 2021 +0000
+++ b/src/main.cpp	Mon May 31 16:44:26 2021 +0000
@@ -10,11 +10,19 @@
 //LIBRARIES
 #include "mbed.h"
 
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/BatteryState.h>
+#include <sensor_msgs/JointState.h>
+
 #include "motor.h"
 #include "encoder.h"
 #include "MotorControl.h"
 
+
 //PIN DEFINITIONS
+#ifdef TARGET_NUCLEO_F401RE
+
 #define     L_MOTOR_PIN     PC_6
 #define     R_MOTOR_PIN     PB_14
 #define     L_MOTOR_DIR     PC_15
@@ -23,49 +31,175 @@
 #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
+
+#else
+#error "Check target against pin definitions"
+#endif
 
 
 //VARIABLES
-#define     MOTOR_FREQ      25000.0         //25 kHz
-#define     MOTOR_TS        0.05            //sample time, ie the time between every PID calculation
-#define     MOTOR_KP        0.00002
-#define     MOTOR_KI        0.0000001
+#define     MOTOR_FREQ      25000.0         // 25 kHz, freq of motor PWM
+#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
+#define     BAT_A_TRIM      1               // Trimming value reporesenting the gain of the voltage monitor
+#define     BAT_B_TRIM      1               // Unit is V
 
 
 //CONSTANTS
-#define     WHEEL_RAD       0.1025         //radius in meters
-#define     PI              3.1415
-#define     ENCODER_PERIOD  0.05            //encoder sampling period in seconds (currently 1/20 sec)
-#define     ENCODER_CPR     980             //clicks per revolution
+#define     WHEEL_RAD       0.1025          // Radius in meters
+#define     WHEEL_BASE      0.400           // Distance between driving wheels in m, affects angular velocity
+#define     PI              3.1415          // Mathematical constant
+#define     ENCODER_PERIOD  0.05            // Encoder sampling period in seconds (currently 1/20 sec)
+#define     ENCODER_CPR     980             // Counts per revolution
 #define     L_MAX_SPEED     1.5
 #define     R_MAX_SPEED     1.5
 
+#define     DEMO            0
+
 /* Functionality Summary
 
 */
 
+// Create node handle
+ros::NodeHandle nh;
+
+// Motor and control objects must be global since they are required in the callback function
+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, NC, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, ((WHEEL_RAD * 2.0 * PI) / ((double)(4 * ENCODER_CPR))));
+MotorControl    rightCtrl   (R_ENC_PIN_A, R_ENC_PIN_B, NC, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, ((WHEEL_RAD * 2.0 * PI) / ((double)(4 * ENCODER_CPR))));
+
+// Callback function for a twist message given by the motion controller
+void messageCb(const geometry_msgs::Twist& msg);
+
+// Subsribe to cmd_vel
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
+
+// Messages used for publishing battery voltage and wheel speeds
+sensor_msgs::BatteryState batA_msg;
+sensor_msgs::BatteryState batB_msg;
+sensor_msgs::JointState wheels_msg;
+
+// Publishers
+ros::Publisher batA_pub("battery_pack_A", &batA_msg);
+ros::Publisher batB_pub("battery_pack_B", &batB_msg);
+ros::Publisher wheels_pub("wheel_speeds", &wheels_msg);
+
+
 int main() {
     
-    //  Constant used by QEI libraries
-    double          enc_const       = ((WHEEL_RAD * 2.0 * PI) / ((double)(4 * ENCODER_CPR)));
-    // Motor PWM frequency
-    double          motT            = 1/MOTOR_FREQ;
-    
     // Initialisation
-    motor           leftMot(L_MOTOR_PIN, L_MOTOR_DIR, motT);
-    //motor           rightMot(R_MOTOR_PIN, R_MOTOR_DIR, motT);
+    nh.initNode();
+    nh.subscribe(sub);
+    
+    // Set up joint state message
+    //wheels_msg.name = ["Left wheel","Right wheel"];
+    wheels_msg.name[0] = "Left wheel";
+    wheels_msg.name[1] = "Right wheel";
+    
+    // Set up battery messages
+    batA_msg.power_supply_technology    =   2;
+    batA_msg.location                   =   "Slot A";
+    batA_msg.design_capacity            =   5.2;
+    batB_msg.power_supply_technology    =   2;
+    batB_msg.location                   =   "Slot B";
+    batB_msg.design_capacity            =   5.2;
     
-    MotorControl    leftCtrl(L_ENC_PIN_A, L_ENC_PIN_B, NC, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_KI, enc_const);
-    //MotorControl    rightCtrl(R_ENC_PIN_A, R_ENC_PIN_B, NC, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_KI, enc_const);
-
+    // Analogue reading pins for battery voltage
+    AnalogIn    batA    (BAT_A_PIN);
+    AnalogIn    batB    (BAT_B_PIN);
+    
+    // Built-in lED used as a connection indicator
+    DigitalOut myled(LED1);
+    myled = 0;
+    
+    // Wait until connected to ROS, while blinking LED
+    while(!nh.connected()) {
+        nh.spinOnce();
+        wait_ms(500);
+        myled = !myled;
+    }
+    myled = 1;
     
-    leftCtrl.setSpeed(0.5);
-    leftCtrl.driveManual();
+    // Just drive at various speeds if the demo flag is set
+    if(DEMO) {
+        nh.loginfo("Running demo script");
+        leftCtrl.setSpeed(0.5);
+        leftCtrl.driveManual();
+        
+        while (true) {
+            leftCtrl.setSpeed(0.5);
+            //ThisThread::sleep_for(3s);
+            leftCtrl.setSpeed(1.2);
+            //ThisThread::sleep_for(3s);
+            nh.spinOnce();
+        }
+    }
     
-    while (true) {
-        leftCtrl.setSpeed(0.5);
-        //ThisThread::sleep_for(3s);
-        leftCtrl.setSpeed(1.2);
-        //ThisThread::sleep_for(3s);
+    // Otherwise initiate main loop
+    else {
+        
+        nh.loginfo("Setup complete");
+        
+        while(true) {
+            // Set built-in LED on if connected to ROS, otherwise off
+            if(nh.connected())
+                myled = 1;
+            else 
+                myled = 0;
+            
+            // Measure battery voltages
+            batA_msg.voltage = batA * BAT_A_TRIM;
+            batB_msg.voltage = batB * BAT_B_TRIM;
+            
+            // Record wheel speeds
+            //wheels_msg.velocity = [leftCtrl.getSpeed(), rightCtrl.getSpeed()];
+            wheels_msg.velocity[0] = leftCtrl.getSpeed();
+            wheels_msg.velocity[1] = rightCtrl.getSpeed();
+            
+            // Publish results
+            batA_pub.publish(&batA_msg);
+            batB_pub.publish(&batB_msg);
+            wheels_pub.publish(&wheels_msg);
+            
+            nh.spinOnce();
+            
+            // Enter a tight for loop for ~1 second
+            for(int i=0; i<1000; i++) {
+                wait_ms(1);
+                nh.spinOnce();
+            }
+        }
     }
 }
+
+
+void messageCb(const geometry_msgs::Twist& msg) {
+    
+    nh.logdebug("Twist callback triggered");
+  
+    /* Calculate v_left and v_right from the v and omega.
+     * v comes as linear.x in the twist message, wile omeage is angular.z.
+     * v is the average of the wheel speeds, while omega is the difference
+     * divided by the wheelbase distance.
+     */
+    
+    /*
+    if (msg.angular.z == 0 && msg.linear.x == 0) {
+        leftCtrl.stop();
+        rightCtrl.stop();
+    }
+    else {
+        */
+        double v_l = msg.linear.x - WHEEL_BASE * 0.5 * msg.angular.z;
+        double v_r = msg.linear.x + WHEEL_BASE * 0.5 * msg.angular.z;
+        
+        leftCtrl.setSpeed(v_l);
+        rightCtrl.setSpeed(v_r);
+        
+    //}
+}