Ion Systems / Mbed 2 deprecated triforce-control

Control code for Triforce robot.

Dependencies:   triforce-esc PwmInRC mbed

main.cpp@0:d6eeeae3c3cb, 2016-11-18 (annotated)

Committer:
IonSystems
Date:
Fri Nov 18 14:22:26 2016 +0000
Revision:
0:d6eeeae3c3cb
Child:
1:026f79cfd378
Working input and bno055 reading. Working on omnimixing.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IonSystems 0:d6eeeae3c3cb 1 /* Debugging defines */
IonSystems 0:d6eeeae3c3cb 2 #define DEBUG_INPUT // Send input values to usb serial
IonSystems 0:d6eeeae3c3cb 3 #define DEBUG_ORIENTATION // Send orientation readings to usb serial
IonSystems 0:d6eeeae3c3cb 4 #define DEBUG_OUTPUT // Send output values to usb serial
IonSystems 0:d6eeeae3c3cb 5 #define PC_DEBUGGING // Output debugging information over usb serial
IonSystems 0:d6eeeae3c3cb 6
IonSystems 0:d6eeeae3c3cb 7 /* Configuration defines */
IonSystems 0:d6eeeae3c3cb 8 //#define HEADING_LOCK // Enable heading lock functionality
IonSystems 0:d6eeeae3c3cb 9 //#define ORIENTATION // Enable orientation functionality
IonSystems 0:d6eeeae3c3cb 10 /* Includes */
IonSystems 0:d6eeeae3c3cb 11
IonSystems 0:d6eeeae3c3cb 12 #include "bno055.h"
IonSystems 0:d6eeeae3c3cb 13 #include "esc.h"
IonSystems 0:d6eeeae3c3cb 14 #include "PwmIn.h"
IonSystems 0:d6eeeae3c3cb 15 #include "mbed.h"
IonSystems 0:d6eeeae3c3cb 16 #include "Triforce.h"
IonSystems 0:d6eeeae3c3cb 17
IonSystems 0:d6eeeae3c3cb 18 /* Set the delay between fresh samples */
IonSystems 0:d6eeeae3c3cb 19 #define BNO055_SAMPLERATE_DELAY_MS (100)
IonSystems 0:d6eeeae3c3cb 20
IonSystems 0:d6eeeae3c3cb 21 /* USB */
IonSystems 0:d6eeeae3c3cb 22 Serial pc(USBTX, USBRX);
IonSystems 0:d6eeeae3c3cb 23
IonSystems 0:d6eeeae3c3cb 24 /* 8 Channel RC input */
IonSystems 0:d6eeeae3c3cb 25 PwmIn rc_channel_1(p5);
IonSystems 0:d6eeeae3c3cb 26 PwmIn rc_channel_2(p6);
IonSystems 0:d6eeeae3c3cb 27 PwmIn rc_channel_3(p7);
IonSystems 0:d6eeeae3c3cb 28 PwmIn rc_channel_4(p8);
IonSystems 0:d6eeeae3c3cb 29 PwmIn rc_channel_5(p11);
IonSystems 0:d6eeeae3c3cb 30 PwmIn rc_channel_6(p12);
IonSystems 0:d6eeeae3c3cb 31 PwmIn rc_channel_7(p13);
IonSystems 0:d6eeeae3c3cb 32 PwmIn rc_channel_8(p14);
IonSystems 0:d6eeeae3c3cb 33
IonSystems 0:d6eeeae3c3cb 34 /* 5 channel ESC output */
IonSystems 0:d6eeeae3c3cb 35 ESC esc_omni_1(p21);
IonSystems 0:d6eeeae3c3cb 36 ESC esc_omni_2(p22);
IonSystems 0:d6eeeae3c3cb 37 ESC esc_omni_3(p23); //LED4
IonSystems 0:d6eeeae3c3cb 38 //ESC esc_weapon_1(p24); //LED3
IonSystems 0:d6eeeae3c3cb 39 //ESC esc_weapon_2(p25); //LED2
IonSystems 0:d6eeeae3c3cb 40
IonSystems 0:d6eeeae3c3cb 41 /* LEDs */
IonSystems 0:d6eeeae3c3cb 42 DigitalOut led1(LED1);
IonSystems 0:d6eeeae3c3cb 43 DigitalOut led2(LED2);
IonSystems 0:d6eeeae3c3cb 44 DigitalOut led3(LED3);
IonSystems 0:d6eeeae3c3cb 45
IonSystems 0:d6eeeae3c3cb 46 /* Initial control positions */
IonSystems 0:d6eeeae3c3cb 47 static struct rc_controls controls = {
IonSystems 0:d6eeeae3c3cb 48 .channel_1 = 50,
IonSystems 0:d6eeeae3c3cb 49 .channel_2 = 50,
IonSystems 0:d6eeeae3c3cb 50 .channel_3 = 50,
IonSystems 0:d6eeeae3c3cb 51 .channel_4 = 0,
IonSystems 0:d6eeeae3c3cb 52 .channel_5 = 0,
IonSystems 0:d6eeeae3c3cb 53 .channel_6 = 0,
IonSystems 0:d6eeeae3c3cb 54 .channel_7 = 0,
IonSystems 0:d6eeeae3c3cb 55 .channel_8 = 0
IonSystems 0:d6eeeae3c3cb 56 };
IonSystems 0:d6eeeae3c3cb 57
IonSystems 0:d6eeeae3c3cb 58 /* Initial output positions */
IonSystems 0:d6eeeae3c3cb 59 static struct rc_outputs outputs = {
IonSystems 0:d6eeeae3c3cb 60 .wheel_1 = 50,
IonSystems 0:d6eeeae3c3cb 61 .wheel_2 = 50,
IonSystems 0:d6eeeae3c3cb 62 .wheel_3 = 50,
IonSystems 0:d6eeeae3c3cb 63 .weapon_motor_1 = 0,
IonSystems 0:d6eeeae3c3cb 64 .weapon_motor_2 = 0
IonSystems 0:d6eeeae3c3cb 65 };
IonSystems 0:d6eeeae3c3cb 66
IonSystems 0:d6eeeae3c3cb 67 /* Direction vector */
IonSystems 0:d6eeeae3c3cb 68 static struct direction_vector direction = {
IonSystems 0:d6eeeae3c3cb 69 .rotation = 50,
IonSystems 0:d6eeeae3c3cb 70 .x_translation = 50,
IonSystems 0:d6eeeae3c3cb 71 .y_translation = 50
IonSystems 0:d6eeeae3c3cb 72 };
IonSystems 0:d6eeeae3c3cb 73
IonSystems 0:d6eeeae3c3cb 74 /** Read control positions from RC receiver
IonSystems 0:d6eeeae3c3cb 75 *
IonSystems 0:d6eeeae3c3cb 76 * Converts control input pulsewidths read in seconds to throttle %
IonSystems 0:d6eeeae3c3cb 77 */
IonSystems 0:d6eeeae3c3cb 78 void read_inputs(){
IonSystems 0:d6eeeae3c3cb 79 controls.channel_1 = convert_pulsewidth(rc_channel_1.pulsewidth());
IonSystems 0:d6eeeae3c3cb 80 controls.channel_2 = convert_pulsewidth(rc_channel_2.pulsewidth());
IonSystems 0:d6eeeae3c3cb 81 controls.channel_3 = convert_pulsewidth(rc_channel_3.pulsewidth());
IonSystems 0:d6eeeae3c3cb 82 controls.channel_4 = convert_pulsewidth(rc_channel_4.pulsewidth());
IonSystems 0:d6eeeae3c3cb 83 controls.channel_5 = convert_pulsewidth(rc_channel_5.pulsewidth());
IonSystems 0:d6eeeae3c3cb 84 controls.channel_6 = convert_pulsewidth(rc_channel_6.pulsewidth());
IonSystems 0:d6eeeae3c3cb 85 controls.channel_7 = convert_pulsewidth(rc_channel_7.pulsewidth());
IonSystems 0:d6eeeae3c3cb 86 controls.channel_8 = convert_pulsewidth(rc_channel_8.pulsewidth());
IonSystems 0:d6eeeae3c3cb 87
IonSystems 0:d6eeeae3c3cb 88 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 0:d6eeeae3c3cb 89 pc.printf("Input: C1: %i, C2: %i, C3: %i, C4: %i, C5: %i, C6: %i, C7: %i, C8: %i\r\n", controls.channel_1, controls.channel_2, controls.channel_3, controls.channel_4, controls.channel_5, controls.channel_6, controls.channel_7, controls.channel_8);
IonSystems 0:d6eeeae3c3cb 90 #endif
IonSystems 0:d6eeeae3c3cb 91 }
IonSystems 0:d6eeeae3c3cb 92
IonSystems 0:d6eeeae3c3cb 93
IonSystems 0:d6eeeae3c3cb 94 /** Output to OmniMixer and ESCs
IonSystems 0:d6eeeae3c3cb 95 *
IonSystems 0:d6eeeae3c3cb 96 * Sends calculated output values to devices
IonSystems 0:d6eeeae3c3cb 97 */
IonSystems 0:d6eeeae3c3cb 98 void output_stage(){
IonSystems 0:d6eeeae3c3cb 99 esc_omni_1.setThrottle(outputs.wheel_1);
IonSystems 0:d6eeeae3c3cb 100 esc_omni_2.setThrottle(outputs.wheel_2);
IonSystems 0:d6eeeae3c3cb 101 esc_omni_3.setThrottle(outputs.wheel_3);
IonSystems 0:d6eeeae3c3cb 102 //esc_weapon_1.setThrottle(outputs.weapon_motor_1);
IonSystems 0:d6eeeae3c3cb 103 //esc_weapon_2.setThrottle(outputs.weapon_motor_2);
IonSystems 0:d6eeeae3c3cb 104
IonSystems 0:d6eeeae3c3cb 105 #if defined (PC_DEBUGGING) && defined (DEBUG_OUTPUT)
IonSystems 0:d6eeeae3c3cb 106 pc.printf("Output Stage: Rotation: %i, X: %i, Y: %i, Weapon1: %i, Weapon2: %i", outputs.wheel_1, outputs.wheel_2, outputs.wheel_3, outputs.weapon_motor_1, outputs.weapon_motor_2);
IonSystems 0:d6eeeae3c3cb 107 #endif
IonSystems 0:d6eeeae3c3cb 108 }
IonSystems 0:d6eeeae3c3cb 109
IonSystems 0:d6eeeae3c3cb 110 /** Init
IonSystems 0:d6eeeae3c3cb 111 *
IonSystems 0:d6eeeae3c3cb 112 * Sets up devices
IonSystems 0:d6eeeae3c3cb 113 * Will loop init for bno055 untill it starts correctly.
IonSystems 0:d6eeeae3c3cb 114 */
IonSystems 0:d6eeeae3c3cb 115 void init(){
IonSystems 0:d6eeeae3c3cb 116 #ifdef ORIENTATION
IonSystems 0:d6eeeae3c3cb 117 init_bno055:
IonSystems 0:d6eeeae3c3cb 118 /* Initialise the sensor */
IonSystems 0:d6eeeae3c3cb 119 if(!initBNO055()){
IonSystems 0:d6eeeae3c3cb 120
IonSystems 0:d6eeeae3c3cb 121 /* There was a problem detecting the BNO055 ... check your connections */
IonSystems 0:d6eeeae3c3cb 122 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 0:d6eeeae3c3cb 123 pc.printf("BNO055 not detected\r\n");
IonSystems 0:d6eeeae3c3cb 124 #endif
IonSystems 0:d6eeeae3c3cb 125 goto init_bno055;
IonSystems 0:d6eeeae3c3cb 126
IonSystems 0:d6eeeae3c3cb 127 } else {
IonSystems 0:d6eeeae3c3cb 128 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 0:d6eeeae3c3cb 129 pc.printf("BNO055 was detected!\r\n");
IonSystems 0:d6eeeae3c3cb 130 #endif
IonSystems 0:d6eeeae3c3cb 131 }
IonSystems 0:d6eeeae3c3cb 132 #endif
IonSystems 0:d6eeeae3c3cb 133 }
IonSystems 0:d6eeeae3c3cb 134
IonSystems 0:d6eeeae3c3cb 135 /** Calculate Orientation
IonSystems 0:d6eeeae3c3cb 136 *
IonSystems 0:d6eeeae3c3cb 137 * Calculates orientation using the BNO055 sensor
IonSystems 0:d6eeeae3c3cb 138 */
IonSystems 0:d6eeeae3c3cb 139 void calculate_orientation(){
IonSystems 0:d6eeeae3c3cb 140
IonSystems 0:d6eeeae3c3cb 141 /* If there is an error then we maintain the same
IonSystems 0:d6eeeae3c3cb 142 * orientation to stop random control flipping */
IonSystems 0:d6eeeae3c3cb 143 if(!bno055Healthy()){
IonSystems 0:d6eeeae3c3cb 144 pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
IonSystems 0:d6eeeae3c3cb 145 }else{
IonSystems 0:d6eeeae3c3cb 146
IonSystems 0:d6eeeae3c3cb 147 /* Read in the Euler angles */
IonSystems 0:d6eeeae3c3cb 148 orientation = getEulerAngles();
IonSystems 0:d6eeeae3c3cb 149
IonSystems 0:d6eeeae3c3cb 150 /* We are upside down in range -30 -> -90 */
IonSystems 0:d6eeeae3c3cb 151 if(orientation.roll < -30 && orientation.roll > -90){
IonSystems 0:d6eeeae3c3cb 152 inverted = true;
IonSystems 0:d6eeeae3c3cb 153 }else{
IonSystems 0:d6eeeae3c3cb 154 inverted = false;
IonSystems 0:d6eeeae3c3cb 155 }
IonSystems 0:d6eeeae3c3cb 156 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 0:d6eeeae3c3cb 157 pc.printf("Roll=%7.2f, Inverted=%s", orientation.roll, inverted ? "true" : "false");
IonSystems 0:d6eeeae3c3cb 158 #endif
IonSystems 0:d6eeeae3c3cb 159 }
IonSystems 0:d6eeeae3c3cb 160
IonSystems 0:d6eeeae3c3cb 161
IonSystems 0:d6eeeae3c3cb 162 }
IonSystems 0:d6eeeae3c3cb 163
IonSystems 0:d6eeeae3c3cb 164 /** Calculate controls
IonSystems 0:d6eeeae3c3cb 165 *
IonSystems 0:d6eeeae3c3cb 166 * Calculates controls based on if the robot is inverted or not
IonSystems 0:d6eeeae3c3cb 167 */
IonSystems 0:d6eeeae3c3cb 168 void calculate_controls(){
IonSystems 0:d6eeeae3c3cb 169 if(inverted){
IonSystems 0:d6eeeae3c3cb 170
IonSystems 0:d6eeeae3c3cb 171 }else{
IonSystems 0:d6eeeae3c3cb 172
IonSystems 0:d6eeeae3c3cb 173 float theta = (float)atan2((double)direction.x_translation, (double)direction.y_translation);
IonSystems 0:d6eeeae3c3cb 174 float magnitude = (float)sqrt((double)((direction.x_translation*direction.x_translation)+(direction.y_translation*direction.y_translation)));
IonSystems 0:d6eeeae3c3cb 175
IonSystems 0:d6eeeae3c3cb 176 if(magnitude > 75.0f){
IonSystems 0:d6eeeae3c3cb 177
IonSystems 0:d6eeeae3c3cb 178 float vx = magnitude * cos(theta);
IonSystems 0:d6eeeae3c3cb 179 float vy = magnitude * sin(theta);
IonSystems 0:d6eeeae3c3cb 180 const float sqrt3o2 = 1.0*sqrt(3.0)/2;
IonSystems 0:d6eeeae3c3cb 181
IonSystems 0:d6eeeae3c3cb 182 float w0 = -vx; // v dot [-1, 0] / 25mm
IonSystems 0:d6eeeae3c3cb 183 float w1 = 0.5*vx - sqrt3o2 * vy; // v dot [1/2, -sqrt(3)/2] / 25mm
IonSystems 0:d6eeeae3c3cb 184 float w2 = 0.5*vx + sqrt3o2 * vy; // v dot [1/2, +sqrt(3)/2] / 25mm
IonSystems 0:d6eeeae3c3cb 185
IonSystems 0:d6eeeae3c3cb 186 float w0_speed = map(w0, 0, 600, 0, 100);
IonSystems 0:d6eeeae3c3cb 187 float w1_speed = map(w1, 0, 600, 0, 100);
IonSystems 0:d6eeeae3c3cb 188 float w2_speed = map(w2, 0, 600, 0, 100);
IonSystems 0:d6eeeae3c3cb 189
IonSystems 0:d6eeeae3c3cb 190 /* Add in rotation */
IonSystems 0:d6eeeae3c3cb 191
IonSystems 0:d6eeeae3c3cb 192 w0_speed += direction.rotation;
IonSystems 0:d6eeeae3c3cb 193 w1_speed += direction.rotation;
IonSystems 0:d6eeeae3c3cb 194 w2_speed += direction.rotation;
IonSystems 0:d6eeeae3c3cb 195
IonSystems 0:d6eeeae3c3cb 196 /* Clamp outputs to correct range */
IonSystems 0:d6eeeae3c3cb 197 outputs.wheel_1 = clamp(w0_speed, 0, 100);
IonSystems 0:d6eeeae3c3cb 198 outputs.wheel_2 = clamp(w1_speed, 0, 100);
IonSystems 0:d6eeeae3c3cb 199 outputs.wheel_3 = clamp(w2_speed, 0, 100);
IonSystems 0:d6eeeae3c3cb 200
IonSystems 0:d6eeeae3c3cb 201
IonSystems 0:d6eeeae3c3cb 202
IonSystems 0:d6eeeae3c3cb 203
IonSystems 0:d6eeeae3c3cb 204 }else{
IonSystems 0:d6eeeae3c3cb 205 outputs.wheel_1 = 50;
IonSystems 0:d6eeeae3c3cb 206 outputs.wheel_2 = 50;
IonSystems 0:d6eeeae3c3cb 207 outputs.wheel_3 = 50;
IonSystems 0:d6eeeae3c3cb 208 }
IonSystems 0:d6eeeae3c3cb 209
IonSystems 0:d6eeeae3c3cb 210
IonSystems 0:d6eeeae3c3cb 211
IonSystems 0:d6eeeae3c3cb 212
IonSystems 0:d6eeeae3c3cb 213 }
IonSystems 0:d6eeeae3c3cb 214 }
IonSystems 0:d6eeeae3c3cb 215
IonSystems 0:d6eeeae3c3cb 216 /** Heading correction
IonSystems 0:d6eeeae3c3cb 217 *
IonSystems 0:d6eeeae3c3cb 218 * Works out which way to spin to return to set heading
IonSystems 0:d6eeeae3c3cb 219 * Based on tiberius turnto code
IonSystems 0:d6eeeae3c3cb 220 *
IonSystems 0:d6eeeae3c3cb 221 * @param speed speed to rotate in percent
IonSystems 0:d6eeeae3c3cb 222 */
IonSystems 0:d6eeeae3c3cb 223 void calculate_heading_correction(float speed, float deadband){
IonSystems 0:d6eeeae3c3cb 224
IonSystems 0:d6eeeae3c3cb 225 /* Sanitize speed */
IonSystems 0:d6eeeae3c3cb 226 if(speed > 100)
IonSystems 0:d6eeeae3c3cb 227 speed = 100;
IonSystems 0:d6eeeae3c3cb 228 else if(speed < 0)
IonSystems 0:d6eeeae3c3cb 229 speed = 0;
IonSystems 0:d6eeeae3c3cb 230
IonSystems 0:d6eeeae3c3cb 231 /* speed is in percent, but controls normal position is at 50% for no movement */
IonSystems 0:d6eeeae3c3cb 232 speed /= 2;
IonSystems 0:d6eeeae3c3cb 233
IonSystems 0:d6eeeae3c3cb 234 /* deadband specifies total angle to ignore, so split for each side */
IonSystems 0:d6eeeae3c3cb 235 deadband = (deadband / 2);
IonSystems 0:d6eeeae3c3cb 236
IonSystems 0:d6eeeae3c3cb 237 /* Normalize headings */
IonSystems 0:d6eeeae3c3cb 238 float n_current_heading = normalize(orientation.heading);
IonSystems 0:d6eeeae3c3cb 239 float n_heading_lock = normalize(heading_lock);
IonSystems 0:d6eeeae3c3cb 240
IonSystems 0:d6eeeae3c3cb 241 pc.printf("Current heading: %f, Desired Heading: %f", n_current_heading, n_heading_lock);
IonSystems 0:d6eeeae3c3cb 242
IonSystems 0:d6eeeae3c3cb 243 /* Calculate error */
IonSystems 0:d6eeeae3c3cb 244 float error = n_current_heading - n_heading_lock;
IonSystems 0:d6eeeae3c3cb 245
IonSystems 0:d6eeeae3c3cb 246 /* Normalize error */
IonSystems 0:d6eeeae3c3cb 247 error = normalize(error);
IonSystems 0:d6eeeae3c3cb 248
IonSystems 0:d6eeeae3c3cb 249 /* Figure out which way to turn */
IonSystems 0:d6eeeae3c3cb 250 if(error > deadband){
IonSystems 0:d6eeeae3c3cb 251
IonSystems 0:d6eeeae3c3cb 252 /* Turning left */
IonSystems 0:d6eeeae3c3cb 253 direction.rotation += speed;
IonSystems 0:d6eeeae3c3cb 254 }else if(error < -deadband){
IonSystems 0:d6eeeae3c3cb 255
IonSystems 0:d6eeeae3c3cb 256 /* Turning right */
IonSystems 0:d6eeeae3c3cb 257 direction.rotation -= speed;
IonSystems 0:d6eeeae3c3cb 258 }
IonSystems 0:d6eeeae3c3cb 259
IonSystems 0:d6eeeae3c3cb 260 }
IonSystems 0:d6eeeae3c3cb 261
IonSystems 0:d6eeeae3c3cb 262 /** Main
IonSystems 0:d6eeeae3c3cb 263 *
IonSystems 0:d6eeeae3c3cb 264 * Main Loop
IonSystems 0:d6eeeae3c3cb 265 */
IonSystems 0:d6eeeae3c3cb 266 int main() {
IonSystems 0:d6eeeae3c3cb 267 #ifdef PC_DEBUGGING
IonSystems 0:d6eeeae3c3cb 268 pc.baud(115200);
IonSystems 0:d6eeeae3c3cb 269 pc.printf("Triforce Control System \r\n");
IonSystems 0:d6eeeae3c3cb 270 #endif
IonSystems 0:d6eeeae3c3cb 271
IonSystems 0:d6eeeae3c3cb 272 init();
IonSystems 0:d6eeeae3c3cb 273
IonSystems 0:d6eeeae3c3cb 274 while(true){
IonSystems 0:d6eeeae3c3cb 275 read_inputs();
IonSystems 0:d6eeeae3c3cb 276
IonSystems 0:d6eeeae3c3cb 277 //calculate_orientation();
IonSystems 0:d6eeeae3c3cb 278 //calculate_controls();
IonSystems 0:d6eeeae3c3cb 279
IonSystems 0:d6eeeae3c3cb 280 #ifdef HEADING_LOCK
IonSystems 0:d6eeeae3c3cb 281 calculate_heading_correction(speed, deadband){
IonSystems 0:d6eeeae3c3cb 282 #endif
IonSystems 0:d6eeeae3c3cb 283 //output_stage();
IonSystems 0:d6eeeae3c3cb 284
IonSystems 0:d6eeeae3c3cb 285 wait_ms(20);
IonSystems 0:d6eeeae3c3cb 286 }
IonSystems 0:d6eeeae3c3cb 287
IonSystems 0:d6eeeae3c3cb 288
IonSystems 0:d6eeeae3c3cb 289
IonSystems 0:d6eeeae3c3cb 290 }
IonSystems 0:d6eeeae3c3cb 291
IonSystems 0:d6eeeae3c3cb 292