Wheel control software for satellite microcontroller running the motors on the Karbor
Dependencies: mbed ros_lib_melodic
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); + + //} +}