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