USNA ES456 Autonomous Vehicles / Mbed 2 deprecated MadPulse_Controller_ros

Madpulse ROS Code

Dependencies:   mbed ServoOut BNO055_fusion ros_lib_kinetic MadPulseIMU ServoIn

main.cpp@5:c24490c61022, 2019-09-12 (annotated)

Committer:
jdawkins
Date:
Thu Sep 12 13:40:43 2019 +0000
Revision:
5:c24490c61022
Parent:
3:82e223a4a4e4
Child:
6:05a5c22cdfc2
Initial Commit of Madpulse Ros Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:42d1dda7d9c0 1 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
jdawkins 0:42d1dda7d9c0 2
jdawkins 5:c24490c61022 3 #define HEARTBEAT_RATE 1.0
jdawkins 5:c24490c61022 4 #define LOG_RATE 20.0
jdawkins 5:c24490c61022 5 #define CTRL_RATE 100.0
jdawkins 5:c24490c61022 6 #define LOOP_RATE 500.0
jdawkins 0:42d1dda7d9c0 7 #define CMD_TIMEOUT 1.0
jdawkins 0:42d1dda7d9c0 8 #define GEAR_RATIO (1/2.75)
jdawkins 0:42d1dda7d9c0 9 #define PI 3.14159
jdawkins 0:42d1dda7d9c0 10 #include "mbed.h"
jdawkins 0:42d1dda7d9c0 11
jdawkins 0:42d1dda7d9c0 12 #include "BNO055.h"
jdawkins 5:c24490c61022 13 #include "ServoIn.h"
jdawkins 5:c24490c61022 14 #include "ServoOut.h"
jdawkins 5:c24490c61022 15 //#include "RC_Channel.h"
jdawkins 5:c24490c61022 16 #include <ros.h>
jdawkins 5:c24490c61022 17 #include <sensor_msgs/Imu.h>
jdawkins 5:c24490c61022 18 #include <geometry_msgs/Twist.h>
jdawkins 0:42d1dda7d9c0 19
jdawkins 0:42d1dda7d9c0 20 DigitalOut status_LED(LED1);
jdawkins 0:42d1dda7d9c0 21 DigitalOut armed_LED(LED2);
jdawkins 0:42d1dda7d9c0 22 DigitalOut auto_LED(LED3);
jdawkins 2:899128d20215 23 DigitalOut imu_LED(LED4);
jdawkins 0:42d1dda7d9c0 24
jdawkins 5:c24490c61022 25 BNO055 imu(p9, p10);
jdawkins 5:c24490c61022 26
jdawkins 5:c24490c61022 27 ServoOut Steer(p26);
jdawkins 5:c24490c61022 28 ServoOut Throttle(p22);
jdawkins 5:c24490c61022 29
jdawkins 5:c24490c61022 30 InterruptIn wheel_sensor(p17);
jdawkins 5:c24490c61022 31
jdawkins 5:c24490c61022 32 //RC_Channel RC[] = {RC_Channel(p15,1), RC_Channel(p16,2)};
jdawkins 5:c24490c61022 33 //RC_Channel RC(p15,1); // instanciate the class
jdawkins 5:c24490c61022 34 //RC_Channel RC(p16,2);
jdawkins 5:c24490c61022 35 ServoIn CH1(p15);
jdawkins 5:c24490c61022 36 ServoIn CH2(p16);
jdawkins 5:c24490c61022 37
jdawkins 5:c24490c61022 38
jdawkins 0:42d1dda7d9c0 39 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 5:c24490c61022 40
jdawkins 5:c24490c61022 41 class XbeeMbedHardware : public MbedHardware
jdawkins 5:c24490c61022 42 {
jdawkins 5:c24490c61022 43 public:
jdawkins 5:c24490c61022 44 XbeeMbedHardware(): MbedHardware(p13, p14, 57600) {};
jdawkins 5:c24490c61022 45 };
jdawkins 5:c24490c61022 46
jdawkins 0:42d1dda7d9c0 47
jdawkins 5:c24490c61022 48 void cmdCallback(const geometry_msgs::Twist& cmd_msg);
jdawkins 5:c24490c61022 49
jdawkins 5:c24490c61022 50 ros::NodeHandle_<XbeeMbedHardware> nh;
jdawkins 5:c24490c61022 51
jdawkins 5:c24490c61022 52 sensor_msgs::Imu imu_msg;
jdawkins 5:c24490c61022 53 ros::Publisher imu_pub("imu",&imu_msg);
jdawkins 5:c24490c61022 54 ros::Subscriber<geometry_msgs::Twist> cmd_sub("cmd_vel", &cmdCallback);
jdawkins 0:42d1dda7d9c0 55
jdawkins 0:42d1dda7d9c0 56
jdawkins 0:42d1dda7d9c0 57
jdawkins 0:42d1dda7d9c0 58 Timer t; // create timer instance
jdawkins 5:c24490c61022 59 Ticker crtlTick;
jdawkins 5:c24490c61022 60 Ticker logTick;
jdawkins 0:42d1dda7d9c0 61
jdawkins 5:c24490c61022 62 float t_imu,t_loop,t_hb,t_cmd,str_cmd,thr_cmd,str,thr,des_psi,des_spd;
jdawkins 5:c24490c61022 63
jdawkins 5:c24490c61022 64 float t_hall, dt_hall,t_run,t_stop,t_log,dt,t_ctrl;
jdawkins 5:c24490c61022 65 bool armed, auto_ctrl,auto_ctrl_old,rc_conn;
jdawkins 0:42d1dda7d9c0 66 float wheel_spd;
jdawkins 5:c24490c61022 67 float arm_clock,auto_clock;
jdawkins 0:42d1dda7d9c0 68 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 5:c24490c61022 69 int cmd_mode;
jdawkins 5:c24490c61022 70
jdawkins 5:c24490c61022 71
jdawkins 5:c24490c61022 72 void wheel_tick_callback()
jdawkins 5:c24490c61022 73 {
jdawkins 5:c24490c61022 74 armed_LED = !armed_LED;
jdawkins 5:c24490c61022 75
jdawkins 5:c24490c61022 76 dt_hall = t.read()-t_hall;
jdawkins 5:c24490c61022 77 t_hall = t.read();
jdawkins 5:c24490c61022 78 }
jdawkins 5:c24490c61022 79
jdawkins 5:c24490c61022 80 void cmdCallback(const geometry_msgs::Twist& cmd_msg)
jdawkins 5:c24490c61022 81 {
jdawkins 5:c24490c61022 82 if(t.read()-t_cmd > 0.2){
jdawkins 5:c24490c61022 83 auto_ctrl = false;
jdawkins 5:c24490c61022 84 }
jdawkins 5:c24490c61022 85 else {
jdawkins 5:c24490c61022 86 auto_ctrl = true;
jdawkins 5:c24490c61022 87 }
jdawkins 5:c24490c61022 88 str_cmd = cmd_msg.angular.z;
jdawkins 5:c24490c61022 89 thr_cmd = cmd_msg.linear.x;
jdawkins 5:c24490c61022 90 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 91 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 92
jdawkins 5:c24490c61022 93 // pc.printf("cmd_vel %f %f\n\r",cmd_msg.linear.x,cmd_msg.angular.z);
jdawkins 5:c24490c61022 94 t_cmd = t.read();
jdawkins 5:c24490c61022 95
jdawkins 5:c24490c61022 96 }
jdawkins 5:c24490c61022 97
jdawkins 5:c24490c61022 98 void controlLoop()
jdawkins 5:c24490c61022 99 {
jdawkins 5:c24490c61022 100 imu.get_angles();
jdawkins 5:c24490c61022 101 imu.get_accel();
jdawkins 5:c24490c61022 102 imu.get_gyro();
jdawkins 5:c24490c61022 103 //imu.get_mag();
jdawkins 5:c24490c61022 104 imu.get_quat();
jdawkins 5:c24490c61022 105
jdawkins 5:c24490c61022 106 if(!auto_ctrl){
jdawkins 5:c24490c61022 107 str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 5:c24490c61022 108 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 5:c24490c61022 109 }
jdawkins 5:c24490c61022 110
jdawkins 5:c24490c61022 111 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 112 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 113 imu_LED = !imu_LED;
jdawkins 5:c24490c61022 114 }
jdawkins 5:c24490c61022 115
jdawkins 5:c24490c61022 116 void logLoop(){
jdawkins 5:c24490c61022 117
jdawkins 5:c24490c61022 118 imu_msg.header.frame_id = "body";
jdawkins 5:c24490c61022 119 imu_msg.orientation.x = imu.quat.x;
jdawkins 5:c24490c61022 120 imu_msg.orientation.y = imu.quat.y;
jdawkins 5:c24490c61022 121 imu_msg.orientation.z = imu.quat.z;
jdawkins 5:c24490c61022 122 imu_msg.orientation.w = imu.quat.w;
jdawkins 5:c24490c61022 123
jdawkins 5:c24490c61022 124 imu_msg.angular_velocity.x = imu.gyro.x;
jdawkins 5:c24490c61022 125 imu_msg.angular_velocity.y = imu.gyro.y;
jdawkins 5:c24490c61022 126 imu_msg.angular_velocity.z = imu.gyro.z;
jdawkins 5:c24490c61022 127
jdawkins 5:c24490c61022 128 imu_msg.linear_acceleration.x = imu.accel.x;
jdawkins 5:c24490c61022 129 imu_msg.linear_acceleration.y = imu.accel.y;
jdawkins 5:c24490c61022 130 imu_msg.linear_acceleration.z = imu.accel.z;
jdawkins 5:c24490c61022 131
jdawkins 5:c24490c61022 132 pc.printf("st %.2f thr %.2f \r\n",str_cmd,thr_cmd);
jdawkins 5:c24490c61022 133
jdawkins 5:c24490c61022 134 imu_pub.publish(&imu_msg);
jdawkins 5:c24490c61022 135
jdawkins 5:c24490c61022 136 }
jdawkins 5:c24490c61022 137
jdawkins 5:c24490c61022 138 float wrapToPi(float ang);
jdawkins 0:42d1dda7d9c0 139
jdawkins 0:42d1dda7d9c0 140 int main()
jdawkins 0:42d1dda7d9c0 141 {
jdawkins 5:c24490c61022 142 NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished)
jdawkins 0:42d1dda7d9c0 143
jdawkins 0:42d1dda7d9c0 144 pc.baud(115200);
jdawkins 5:c24490c61022 145 // xbee.baud(115200);
jdawkins 5:c24490c61022 146 // logTick.attach(&logLoop,0.1);
jdawkins 5:c24490c61022 147 // crtlTick.attach(&controlLoop,0.02);
jdawkins 0:42d1dda7d9c0 148
jdawkins 5:c24490c61022 149 wheel_sensor.rise(&wheel_tick_callback);
jdawkins 5:c24490c61022 150
jdawkins 0:42d1dda7d9c0 151 str_cond = false;
jdawkins 0:42d1dda7d9c0 152 thr_cond = false;
jdawkins 0:42d1dda7d9c0 153 armed = false;
jdawkins 0:42d1dda7d9c0 154 auto_ctrl = false;
jdawkins 0:42d1dda7d9c0 155 run_ctrl = false;
jdawkins 0:42d1dda7d9c0 156 log_data = false;
jdawkins 0:42d1dda7d9c0 157
jdawkins 0:42d1dda7d9c0 158 t.start();
jdawkins 0:42d1dda7d9c0 159 t_imu = t.read();
jdawkins 5:c24490c61022 160 t_ctrl = t.read();
jdawkins 5:c24490c61022 161 dt = 0;
jdawkins 0:42d1dda7d9c0 162 t_cmd = 0;
jdawkins 0:42d1dda7d9c0 163
jdawkins 0:42d1dda7d9c0 164 status_LED = 1;
jdawkins 0:42d1dda7d9c0 165
jdawkins 0:42d1dda7d9c0 166 if(imu.check()) {
jdawkins 5:c24490c61022 167
jdawkins 0:42d1dda7d9c0 168 pc.printf("BNO055 connected\r\n");
jdawkins 0:42d1dda7d9c0 169 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:42d1dda7d9c0 170 imu.SetExternalCrystal(1);
jdawkins 0:42d1dda7d9c0 171 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 5:c24490c61022 172 imu.set_angle_units(RADIANS);
jdawkins 3:82e223a4a4e4 173 imu.set_accel_units(MPERSPERS);
jdawkins 5:c24490c61022 174 imu.set_anglerate_units(RAD_PER_SEC);
jdawkins 3:82e223a4a4e4 175 imu.setoutputformat(WINDOWS);
jdawkins 2:899128d20215 176 imu.set_mapping(2);
jdawkins 3:82e223a4a4e4 177
jdawkins 5:c24490c61022 178 /* while(int(imu.calib) < 0xCF) {
jdawkins 5:c24490c61022 179 pc.printf("Calibration = %x.\n\r",imu.calib);
jdawkins 5:c24490c61022 180 imu.get_calib();
jdawkins 5:c24490c61022 181 wait(0.5);
jdawkins 5:c24490c61022 182 imu_LED = !imu_LED;
jdawkins 5:c24490c61022 183 }*/
jdawkins 5:c24490c61022 184 imu_LED = 1;
jdawkins 5:c24490c61022 185
jdawkins 5:c24490c61022 186
jdawkins 0:42d1dda7d9c0 187 } else {
jdawkins 0:42d1dda7d9c0 188 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 0:42d1dda7d9c0 189 status_LED = 1;
jdawkins 0:42d1dda7d9c0 190 armed_LED = 1;
jdawkins 2:899128d20215 191 imu_LED = 1;
jdawkins 0:42d1dda7d9c0 192 auto_LED = 1;
jdawkins 0:42d1dda7d9c0 193 while(1) {
jdawkins 0:42d1dda7d9c0 194 status_LED = !status_LED;
jdawkins 0:42d1dda7d9c0 195 armed_LED = !armed_LED;
jdawkins 2:899128d20215 196 imu_LED = !imu_LED;
jdawkins 0:42d1dda7d9c0 197 auto_LED = !auto_LED;
jdawkins 0:42d1dda7d9c0 198 wait(0.5);
jdawkins 0:42d1dda7d9c0 199 }
jdawkins 0:42d1dda7d9c0 200 }
jdawkins 5:c24490c61022 201 pc.printf("ES456 Vehicle Control\r\n");
jdawkins 5:c24490c61022 202
jdawkins 5:c24490c61022 203 nh.initNode();
jdawkins 5:c24490c61022 204 nh.advertise(imu_pub);
jdawkins 5:c24490c61022 205 nh.subscribe(cmd_sub);
jdawkins 0:42d1dda7d9c0 206
jdawkins 5:c24490c61022 207
jdawkins 5:c24490c61022 208 while(1) {
jdawkins 5:c24490c61022 209
jdawkins 5:c24490c61022 210
jdawkins 5:c24490c61022 211 if(t.read()-t_imu > (1/HEARTBEAT_RATE)) {
jdawkins 5:c24490c61022 212 // pc.printf("RC0: %4d RC1: %4d ", RC[0].read(), RC[1].read());
jdawkins 5:c24490c61022 213 status_LED=!status_LED;
jdawkins 5:c24490c61022 214 t_imu = t.read();
jdawkins 5:c24490c61022 215 } // if t.read
jdawkins 5:c24490c61022 216
jdawkins 5:c24490c61022 217 if(t.read()-t_ctrl > (1/CTRL_RATE)){
jdawkins 5:c24490c61022 218 controlLoop();
jdawkins 5:c24490c61022 219 }
jdawkins 5:c24490c61022 220
jdawkins 5:c24490c61022 221 if(t.read()-t_log > (1/LOG_RATE)){
jdawkins 5:c24490c61022 222 logLoop();
jdawkins 5:c24490c61022 223 }
jdawkins 0:42d1dda7d9c0 224
jdawkins 3:82e223a4a4e4 225
jdawkins 3:82e223a4a4e4 226
jdawkins 5:c24490c61022 227 // Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 228 //Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 5:c24490c61022 229
jdawkins 5:c24490c61022 230 nh.spinOnce();
jdawkins 5:c24490c61022 231 wait_us(10);
jdawkins 0:42d1dda7d9c0 232 } // while (1)
jdawkins 0:42d1dda7d9c0 233
jdawkins 0:42d1dda7d9c0 234 } // main
jdawkins 0:42d1dda7d9c0 235
jdawkins 0:42d1dda7d9c0 236
jdawkins 5:c24490c61022 237 float wrapToPi(float ang)
jdawkins 5:c24490c61022 238 {
jdawkins 0:42d1dda7d9c0 239
jdawkins 5:c24490c61022 240 if(ang > PI) {
jdawkins 5:c24490c61022 241
jdawkins 5:c24490c61022 242 ang = ang - 2*PI;
jdawkins 5:c24490c61022 243 }
jdawkins 5:c24490c61022 244 if (ang < -PI) {
jdawkins 5:c24490c61022 245 ang = ang + 2*PI;
jdawkins 5:c24490c61022 246 }
jdawkins 5:c24490c61022 247
jdawkins 5:c24490c61022 248 return ang;
jdawkins 5:c24490c61022 249 }
jdawkins 5:c24490c61022 250
jdawkins 5:c24490c61022 251