Wheel control software for satellite microcontroller running the motors on the Karbor
Dependencies: mbed ros_lib_melodic
src/main.cpp
- Committer:
- krogedal
- Date:
- 2021-05-31
- Revision:
- 3:4b6080e86761
- Parent:
- 2:aeaa1c1e0d33
- Child:
- 5:44b2454a5eea
File content as of revision 3:4b6080e86761:
/* 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 "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 #define R_MOTOR_DIR PC_14 #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 #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, 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() { // Initialisation 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; // 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; // 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(); } } // 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); //} }