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?
User | Revision | Line number | New 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 |