main.cpp

00001 /** @file main.cpp
00002  * @author Simon Krogedal
00003  * @version 0.1
00004  */
00005 
00006 /* Karbot motor controller
00007  * Written by Simon Krogedal
00008  * 26/05/21
00009  * Team 9 4th Year project
00010  * 
00011  * for NUCLEO-F401RE
00012  * 
00013  */
00014 
00015 //LIBRARIES
00016 #include "mbed.h"
00017 
00018 #include <ros.h>
00019 #include <geometry_msgs/Twist.h>
00020 #include <sensor_msgs/BatteryState.h>
00021 #include <sensor_msgs/JointState.h>
00022 #include <std_msgs/Float32.h>
00023 
00024 #include "motor.h"
00025 #include "encoder.h"
00026 #include "MotorControl.h"
00027 
00028 
00029 //PIN DEFINITIONS
00030 #ifdef TARGET_NUCLEO_F401RE
00031 
00032 #define     R_MOTOR_PIN     PA_8
00033 #define     L_MOTOR_PIN     PC_7
00034 #define     R_MOTOR_DIR     PB_10
00035 #define     L_MOTOR_DIR     PA_9
00036 
00037 #define     R_ENC_PIN_A     PB_5    // Blue
00038 #define     R_ENC_PIN_B     PB_4    // Purple
00039 #define     L_ENC_PIN_A     PB_3
00040 #define     L_ENC_PIN_B     PC_13
00041 
00042 /* These pin definitions were used in ESP, it might be smart to go use those
00043 #define     L_ENC_PIN_A     PA_14
00044 #define     L_ENC_PIN_B     PB_15
00045 #define     R_ENC_PIN_A     PA_13
00046 #define     R_ENC_PIN_B     PB_1
00047 */
00048 
00049 #define     BAT_A_PIN       PA_0
00050 #define     BAT_B_PIN       PA_1
00051 
00052 #else
00053 #error "Check target against pin definitions"
00054 #endif
00055 
00056 
00057 //VARIABLES
00058 #define     MOTOR_FREQ      20000.0         // 20 kHz, freq of motor PWM, max for driver board
00059 #define     MOTOR_TS        0.05            // Sample time, ie the time between every PID calculation
00060 #define     MOTOR_KP        0.00002         // Loop motor gain
00061 #define     MOTOR_TI        200             // Integral motor gain
00062 #define     BAT_A_TRIM      1               // Trimming value reporesenting the gain of the voltage monitor
00063 #define     BAT_B_TRIM      1               // Unit is V
00064 
00065 
00066 //CONSTANTS
00067 #define     WHEEL_RAD       0.1025          // Radius in meters
00068 #define     WHEEL_BASE      0.400           // Distance between driving wheels in m, affects angular velocity
00069 #define     PI              3.1415          // Mathematical constant
00070 #define     ENCODER_PERIOD  0.05            // Encoder sampling period in seconds (currently 1/20 sec)
00071 #define     ENCODER_CPR     980             // Counts per revolution
00072 #define     L_MAX_SPEED     1.5
00073 #define     R_MAX_SPEED     1.5
00074 
00075 #define     DEMO            0
00076 
00077 /* Functionality Summary
00078 
00079 */
00080 
00081 // Create node handle
00082 ros::NodeHandle nh;
00083 
00084 // Motor and control objects must be global since they are required in the callback function
00085 motor           leftMot     (L_MOTOR_PIN, L_MOTOR_DIR, 1/MOTOR_FREQ);
00086 motor           rightMot    (R_MOTOR_PIN, R_MOTOR_DIR, 1/MOTOR_FREQ);
00087 
00088 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);
00089 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);
00090 
00091 // Callback function for a twist message given by the motion controller
00092 void messageCb(const geometry_msgs::Twist& msg);
00093 
00094 // Subsribe to cmd_vel
00095 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
00096 
00097 // Messages used for publishing battery voltage and wheel speeds
00098 sensor_msgs::BatteryState batA_msg;
00099 sensor_msgs::BatteryState batB_msg;
00100 sensor_msgs::JointState wheels_msg;
00101 
00102 std_msgs::Float32 left_wheel_speed_msg;
00103 std_msgs::Float32 right_wheel_speed_msg;
00104 
00105 // Publishers
00106 ros::Publisher batA_pub("battery_pack_A", &batA_msg);
00107 ros::Publisher batB_pub("battery_pack_B", &batB_msg);
00108 ros::Publisher wheels_pub("wheel_speeds", &wheels_msg);
00109 ros::Publisher left_wheel_pub("left_wheel_speed", &left_wheel_speed_msg);
00110 ros::Publisher right_wheel_pub("right_wheel_speed", &right_wheel_speed_msg);
00111 
00112 
00113 int main() {
00114     
00115     // Initialisation
00116     nh.initNode();
00117 
00118     
00119     // Set up joint state message
00120     wheels_msg.name_length = 2;             // In these libs, lenght must be set
00121     wheels_msg.velocity_length = 2;         // manually for each term
00122     
00123     // Name joints
00124     char *jointNames[] = {"Left wheel","Right wheel"};
00125     wheels_msg.name = jointNames;
00126     
00127     // Set up battery messages
00128     batA_msg.power_supply_technology    =   2;
00129     batA_msg.location                   =   "Slot A";
00130     batA_msg.design_capacity            =   5.2;
00131     batB_msg.power_supply_technology    =   2;
00132     batB_msg.location                   =   "Slot B";
00133     batB_msg.design_capacity            =   5.2;
00134     
00135     // Analogue reading pins for battery voltage
00136     AnalogIn    batA    (BAT_A_PIN);
00137     AnalogIn    batB    (BAT_B_PIN);
00138     
00139     // Built-in lED used as a connection indicator
00140     DigitalOut myled(LED1);
00141     myled = 0;
00142     
00143     
00144     // Just drive at various speeds if the demo flag is set
00145     if(DEMO) {
00146         nh.loginfo("Running demo script");
00147         
00148         leftCtrl.setSpeed(0.5);
00149         leftCtrl.drive();
00150         
00151         nh.advertise(batA_pub);
00152         nh.advertise(batB_pub);
00153         nh.advertise(wheels_pub);
00154         
00155         while (true) {
00156             leftCtrl.setSpeed(0.2);
00157             myled = !myled;
00158             wait(1);
00159             leftCtrl.setSpeed(1.2);
00160             myled = !myled;
00161             wait(1);
00162             
00163             // Record wheel speeds
00164             wheels_msg.velocity[0] = leftCtrl.getSpeed();
00165             wheels_msg.velocity[1] = rightCtrl.getSpeed();
00166             
00167             // Publish results
00168             batA_pub.publish(&batA_msg);
00169             batB_pub.publish(&batB_msg);
00170             wheels_pub.publish(&wheels_msg);
00171             
00172             nh.spinOnce();
00173             
00174         }
00175     }
00176     
00177     // Otherwise initiate main loop
00178     else {
00179         
00180         // Wait until connected to ROS, while blinking LED
00181         while(!nh.connected()) {
00182             nh.spinOnce();
00183             wait_ms(500);
00184             myled = !myled;
00185         }
00186         myled = 1;
00187         
00188         nh.subscribe(sub);
00189         nh.advertise(batA_pub);
00190         nh.advertise(batB_pub);
00191         nh.advertise(wheels_pub);
00192         nh.advertise(left_wheel_pub);
00193         nh.advertise(right_wheel_pub);
00194         nh.loginfo("Setup complete");
00195         
00196         //leftCtrl.start();
00197         //rightCtrl.start();
00198         
00199         // Main loop
00200         while(true) {
00201             // Set built-in LED on if connected to ROS, otherwise off
00202             if(nh.connected())
00203                 myled = 1;
00204             else 
00205                 myled = 0;
00206             
00207             // Measure battery voltages
00208             batA_msg.voltage = batA * BAT_A_TRIM;
00209             batB_msg.voltage = batB * BAT_B_TRIM;
00210             
00211             // Record wheel speeds
00212             //wheels_msg.velocity = [leftCtrl.getSpeed(), rightCtrl.getSpeed()];
00213             wheels_msg.velocity[0] = leftCtrl.getSpeed();
00214             wheels_msg.velocity[1] = rightCtrl.getSpeed();
00215             
00216             left_wheel_speed_msg.data = leftCtrl.getSpeed();
00217             right_wheel_speed_msg.data = rightCtrl.getSpeed();
00218             
00219             
00220             // Publish results
00221             batA_pub.publish(&batA_msg);
00222             batB_pub.publish(&batB_msg);
00223             wheels_pub.publish(&wheels_msg);
00224             
00225             left_wheel_pub.publish(&left_wheel_speed_msg);
00226             right_wheel_pub.publish(&right_wheel_speed_msg);
00227             
00228             nh.spinOnce();
00229             
00230             // Enter a tight for loop for ~1 second
00231             for(int i=0; i<1000; i++) {
00232                 wait_ms(1);
00233                 nh.spinOnce();
00234             }
00235         }
00236     }
00237 }
00238 
00239 
00240 void messageCb(const geometry_msgs::Twist& msg) {
00241     
00242     nh.loginfo("Twist callback triggered");
00243   
00244     /* Calculate v_left and v_right from the v and omega.
00245      * v comes as linear.x in the twist message, wile omeage is angular.z.
00246      * v is the average of the wheel speeds, while omega is the difference
00247      * divided by the wheelbase distance.
00248      */
00249     
00250     
00251     if (msg.angular.z == 0 && msg.linear.x == 0) {
00252         leftCtrl.stop();
00253         rightCtrl.stop();
00254     }
00255     else {
00256         
00257         double v_l = msg.linear.x - WHEEL_BASE * 0.5 * msg.angular.z;
00258         double v_r = msg.linear.x + WHEEL_BASE * 0.5 * msg.angular.z;
00259         
00260         leftCtrl.setSpeed(v_l);
00261         rightCtrl.setSpeed(v_r);
00262         
00263         leftCtrl.drive();
00264         rightCtrl.drive();
00265         
00266     }
00267 }