Wheel control software for satellite microcontroller running the motors on the Karbor
Dependencies: mbed ros_lib_melodic
src/main.cpp
- Committer:
- krogedal
- Date:
- 2021-06-08
- Revision:
- 5:44b2454a5eea
- Parent:
- 3:4b6080e86761
- Child:
- 6:ed47deb76adf
File content as of revision 5:44b2454a5eea:
/** @file main.cpp */ /* Karbot motor controller * Written by Simon Krogedal * 26/05/21 * Team 9 4th Year project * * for NUCLEO-F401RE * */ //LIBRARIES #include "mbed.h" #include <ros.h> #include <geometry_msgs/Twist.h> #include <sensor_msgs/BatteryState.h> #include <sensor_msgs/JointState.h> #include <std_msgs/Float32.h> #include "motor.h" #include "encoder.h" #include "MotorControl.h" //PIN DEFINITIONS #ifdef TARGET_NUCLEO_F401RE #define R_MOTOR_PIN PA_8 #define L_MOTOR_PIN PC_7 #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 L_ENC_PIN_A PB_3 #define L_ENC_PIN_B PC_13 #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, 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 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, 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); // 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; std_msgs::Float32 left_wheel_speed_msg; std_msgs::Float32 right_wheel_speed_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); ros::Publisher left_wheel_pub("left_wheel_speed", &left_wheel_speed_msg); ros::Publisher right_wheel_pub("right_wheel_speed", &right_wheel_speed_msg); int main() { // Initialisation nh.initNode(); // 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; // 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; // Just drive at various speeds if the demo flag is set if(DEMO) { nh.loginfo("Running demo script"); leftCtrl.setSpeed(0.5); leftCtrl.drive(); nh.advertise(batA_pub); nh.advertise(batB_pub); nh.advertise(wheels_pub); while (true) { leftCtrl.setSpeed(0.2); myled = !myled; wait(1); leftCtrl.setSpeed(1.2); myled = !myled; wait(1); // Record wheel speeds 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(); } } // Otherwise initiate main loop else { // Wait until connected to ROS, while blinking LED while(!nh.connected()) { nh.spinOnce(); wait_ms(500); myled = !myled; } myled = 1; nh.subscribe(sub); nh.advertise(batA_pub); nh.advertise(batB_pub); nh.advertise(wheels_pub); nh.advertise(left_wheel_pub); nh.advertise(right_wheel_pub); nh.loginfo("Setup complete"); leftCtrl.start(); rightCtrl.start(); // Main loop 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(); left_wheel_speed_msg.data = leftCtrl.getSpeed(); right_wheel_speed_msg.data = rightCtrl.getSpeed(); // Publish results batA_pub.publish(&batA_msg); 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); 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.loginfo("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); leftCtrl.drive(); rightCtrl.drive(); } }