Ion Systems / Mbed 2 deprecated triforce-control

Control code for Triforce robot.

Dependencies:   triforce-esc PwmInRC mbed

main.cpp@2:4e086a77e769, 2017-05-05 (annotated)

Committer:
IonSystems
Date:
Fri May 05 14:50:05 2017 +0000
Revision:
2:4e086a77e769
Parent:
1:026f79cfd378 
Forked ESC library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IonSystems 0:d6eeeae3c3cb 1 /* Includes */
IonSystems 0:d6eeeae3c3cb 2 #include "bno055.h"
IonSystems 0:d6eeeae3c3cb 3 #include "esc.h"
IonSystems 0:d6eeeae3c3cb 4 #include "PwmIn.h"
IonSystems 0:d6eeeae3c3cb 5 #include "mbed.h"
IonSystems 0:d6eeeae3c3cb 6 #include "Triforce.h"
IonSystems 1:026f79cfd378 7 #include "math.h"
IonSystems 0:d6eeeae3c3cb 8
IonSystems 0:d6eeeae3c3cb 9 /* USB */
IonSystems 0:d6eeeae3c3cb 10 Serial pc(USBTX, USBRX);
IonSystems 0:d6eeeae3c3cb 11
IonSystems 0:d6eeeae3c3cb 12 /* 8 Channel RC input */
IonSystems 1:026f79cfd378 13 PwmIn rc_channel_1(p5); // Roll
IonSystems 1:026f79cfd378 14 PwmIn rc_channel_2(p6); // Pitch
IonSystems 1:026f79cfd378 15 PwmIn rc_channel_3(p7); // Throttle
IonSystems 1:026f79cfd378 16 PwmIn rc_channel_4(p8); // Yaw
IonSystems 1:026f79cfd378 17 PwmIn rc_channel_5(p11); // Switch R front
IonSystems 1:026f79cfd378 18 PwmIn rc_channel_6(p12); // Dial R
IonSystems 1:026f79cfd378 19 PwmIn rc_channel_7(p13); // Dial L
IonSystems 1:026f79cfd378 20 PwmIn rc_channel_8(p14); // Switch L Back
IonSystems 0:d6eeeae3c3cb 21
IonSystems 0:d6eeeae3c3cb 22 /* 5 channel ESC output */
IonSystems 1:026f79cfd378 23 ESC esc_omni_1(p21, 20, 1500);
IonSystems 1:026f79cfd378 24 ESC esc_omni_2(p22, 20, 1500);
IonSystems 1:026f79cfd378 25 ESC esc_omni_3(p23, 20, 1500); //LED4
IonSystems 1:026f79cfd378 26 ESC esc_weapon_1(p24); //LED3
IonSystems 1:026f79cfd378 27 ESC esc_weapon_2(p25); //LED2
IonSystems 0:d6eeeae3c3cb 28
IonSystems 0:d6eeeae3c3cb 29 /* LEDs */
IonSystems 0:d6eeeae3c3cb 30 DigitalOut led1(LED1);
IonSystems 0:d6eeeae3c3cb 31
IonSystems 0:d6eeeae3c3cb 32 /* Initial control positions */
IonSystems 0:d6eeeae3c3cb 33 static struct rc_controls controls = {
IonSystems 0:d6eeeae3c3cb 34 .channel_1 = 50,
IonSystems 0:d6eeeae3c3cb 35 .channel_2 = 50,
IonSystems 0:d6eeeae3c3cb 36 .channel_3 = 50,
IonSystems 0:d6eeeae3c3cb 37 .channel_4 = 0,
IonSystems 0:d6eeeae3c3cb 38 .channel_5 = 0,
IonSystems 0:d6eeeae3c3cb 39 .channel_6 = 0,
IonSystems 0:d6eeeae3c3cb 40 .channel_7 = 0,
IonSystems 0:d6eeeae3c3cb 41 .channel_8 = 0
IonSystems 0:d6eeeae3c3cb 42 };
IonSystems 0:d6eeeae3c3cb 43
IonSystems 0:d6eeeae3c3cb 44 /* Initial output positions */
IonSystems 0:d6eeeae3c3cb 45 static struct rc_outputs outputs = {
IonSystems 0:d6eeeae3c3cb 46 .wheel_1 = 50,
IonSystems 0:d6eeeae3c3cb 47 .wheel_2 = 50,
IonSystems 0:d6eeeae3c3cb 48 .wheel_3 = 50,
IonSystems 0:d6eeeae3c3cb 49 .weapon_motor_1 = 0,
IonSystems 0:d6eeeae3c3cb 50 .weapon_motor_2 = 0
IonSystems 0:d6eeeae3c3cb 51 };
IonSystems 0:d6eeeae3c3cb 52
IonSystems 0:d6eeeae3c3cb 53 /* Direction vector */
IonSystems 0:d6eeeae3c3cb 54 static struct direction_vector direction = {
IonSystems 0:d6eeeae3c3cb 55 .rotation = 50,
IonSystems 0:d6eeeae3c3cb 56 .x_translation = 50,
IonSystems 0:d6eeeae3c3cb 57 .y_translation = 50
IonSystems 0:d6eeeae3c3cb 58 };
IonSystems 0:d6eeeae3c3cb 59
IonSystems 0:d6eeeae3c3cb 60
IonSystems 1:026f79cfd378 61 /******************************************************************************/
IonSystems 0:d6eeeae3c3cb 62
IonSystems 0:d6eeeae3c3cb 63
IonSystems 0:d6eeeae3c3cb 64 /** Init
IonSystems 0:d6eeeae3c3cb 65 *
IonSystems 0:d6eeeae3c3cb 66 * Sets up devices
IonSystems 0:d6eeeae3c3cb 67 * Will loop init for bno055 untill it starts correctly.
IonSystems 0:d6eeeae3c3cb 68 */
IonSystems 0:d6eeeae3c3cb 69 void init(){
IonSystems 0:d6eeeae3c3cb 70 #ifdef ORIENTATION
IonSystems 0:d6eeeae3c3cb 71 init_bno055:
IonSystems 0:d6eeeae3c3cb 72 /* Initialise the sensor */
IonSystems 0:d6eeeae3c3cb 73 if(!initBNO055()){
IonSystems 0:d6eeeae3c3cb 74
IonSystems 0:d6eeeae3c3cb 75 /* There was a problem detecting the BNO055 ... check your connections */
IonSystems 0:d6eeeae3c3cb 76 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 0:d6eeeae3c3cb 77 pc.printf("BNO055 not detected\r\n");
IonSystems 0:d6eeeae3c3cb 78 #endif
IonSystems 0:d6eeeae3c3cb 79 goto init_bno055;
IonSystems 0:d6eeeae3c3cb 80
IonSystems 0:d6eeeae3c3cb 81 } else {
IonSystems 0:d6eeeae3c3cb 82 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 0:d6eeeae3c3cb 83 pc.printf("BNO055 was detected!\r\n");
IonSystems 0:d6eeeae3c3cb 84 #endif
IonSystems 0:d6eeeae3c3cb 85 }
IonSystems 0:d6eeeae3c3cb 86 #endif
IonSystems 0:d6eeeae3c3cb 87 }
IonSystems 0:d6eeeae3c3cb 88
IonSystems 1:026f79cfd378 89 void reset_outputs(){
IonSystems 1:026f79cfd378 90 outputs.wheel_1 = 50;
IonSystems 1:026f79cfd378 91 outputs.wheel_2 = 50;
IonSystems 1:026f79cfd378 92 outputs.wheel_3 = 50;
IonSystems 1:026f79cfd378 93 }
IonSystems 1:026f79cfd378 94
IonSystems 1:026f79cfd378 95
IonSystems 1:026f79cfd378 96 /** Read control positions from RC receiver
IonSystems 1:026f79cfd378 97 *
IonSystems 1:026f79cfd378 98 * Converts control input pulsewidths read in seconds to throttle %
IonSystems 1:026f79cfd378 99 */
IonSystems 1:026f79cfd378 100 void read_inputs(){
IonSystems 1:026f79cfd378 101
IonSystems 1:026f79cfd378 102
IonSystems 1:026f79cfd378 103 /* Check for armed status */
IonSystems 1:026f79cfd378 104
IonSystems 1:026f79cfd378 105
IonSystems 1:026f79cfd378 106 if(!armed || just_armed){
IonSystems 1:026f79cfd378 107 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 108 if(arming_count == 0){
IonSystems 1:026f79cfd378 109
IonSystems 1:026f79cfd378 110 pc.printf("MOVE STICKS TO ARMING POSITIONS AND HOLD TO ARM \r\n");
IonSystems 1:026f79cfd378 111 }
IonSystems 1:026f79cfd378 112 #endif
IonSystems 1:026f79cfd378 113
IonSystems 1:026f79cfd378 114 failsafe = true;
IonSystems 1:026f79cfd378 115 int c1 = convert_pulsewidth(rc_channel_1.pulsewidth());
IonSystems 1:026f79cfd378 116 int c2 = convert_pulsewidth(rc_channel_2.pulsewidth());
IonSystems 1:026f79cfd378 117 int c3 = convert_pulsewidth(rc_channel_3.pulsewidth());
IonSystems 1:026f79cfd378 118 int c4 = convert_pulsewidth(rc_channel_4.pulsewidth());
IonSystems 1:026f79cfd378 119 pc.printf("Input: C1: %i \t C2: %i \t C3: %i \t C4: %i \r\n", c1, c2, c3, c4);
IonSystems 1:026f79cfd378 120
IonSystems 1:026f79cfd378 121 if(c1 > RC_ARM_CHANNEL_1 && c2 > RC_ARM_CHANNEL_2 && c3 < RC_ARM_CHANNEL_3 && c4 < RC_ARM_CHANNEL_4){
IonSystems 1:026f79cfd378 122
IonSystems 1:026f79cfd378 123 if(!armed){
IonSystems 1:026f79cfd378 124 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 125 pc.printf("ARMING %i\r\n", arming_count);
IonSystems 1:026f79cfd378 126 #endif
IonSystems 1:026f79cfd378 127 arming_count++;
IonSystems 1:026f79cfd378 128 wait_ms(1000);
IonSystems 1:026f79cfd378 129 if(arming_count > RC_ARM_DELAY){
IonSystems 1:026f79cfd378 130 armed = true;
IonSystems 1:026f79cfd378 131 just_armed = true;
IonSystems 1:026f79cfd378 132 }
IonSystems 1:026f79cfd378 133 }else{
IonSystems 1:026f79cfd378 134 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 135 pc.printf("ARMED - CENTER CONTROLS \r\n");
IonSystems 1:026f79cfd378 136 #endif
IonSystems 1:026f79cfd378 137 }
IonSystems 1:026f79cfd378 138 } else if(just_armed){
IonSystems 1:026f79cfd378 139 /* Wait for controls to be centered to avoid unexpected movement */
IonSystems 1:026f79cfd378 140 if(BETWEEN(c1, 45, 55) && BETWEEN(c2, 45, 55) && c3 < RC_ARM_CHANNEL_3 && BETWEEN(c4, 45, 55)){
IonSystems 1:026f79cfd378 141 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 142 pc.printf("CONTROLS CENTERED DISABLING ARM LOCK\r\n");
IonSystems 1:026f79cfd378 143 #endif
IonSystems 1:026f79cfd378 144 just_armed = false;
IonSystems 1:026f79cfd378 145 failsafe_timer = 0;
IonSystems 1:026f79cfd378 146 }
IonSystems 1:026f79cfd378 147
IonSystems 1:026f79cfd378 148 }else {
IonSystems 1:026f79cfd378 149 arming_count = 0;
IonSystems 1:026f79cfd378 150 }
IonSystems 1:026f79cfd378 151
IonSystems 1:026f79cfd378 152
IonSystems 1:026f79cfd378 153 }else{
IonSystems 1:026f79cfd378 154
IonSystems 1:026f79cfd378 155
IonSystems 1:026f79cfd378 156
IonSystems 1:026f79cfd378 157
IonSystems 1:026f79cfd378 158
IonSystems 1:026f79cfd378 159 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 160 pc.printf("******************ARMED****************** \r\n");
IonSystems 1:026f79cfd378 161 #endif
IonSystems 1:026f79cfd378 162
IonSystems 1:026f79cfd378 163 controls.channel_1 = convert_pulsewidth(rc_channel_1.pulsewidth());
IonSystems 1:026f79cfd378 164 controls.channel_2 = convert_pulsewidth(rc_channel_2.pulsewidth());
IonSystems 1:026f79cfd378 165 controls.channel_3 = convert_pulsewidth(rc_channel_3.pulsewidth());
IonSystems 1:026f79cfd378 166 controls.channel_4 = convert_pulsewidth(rc_channel_4.pulsewidth());
IonSystems 1:026f79cfd378 167 controls.channel_5 = convert_pulsewidth(rc_channel_5.pulsewidth());
IonSystems 1:026f79cfd378 168 controls.channel_6 = convert_pulsewidth(rc_channel_6.pulsewidth());
IonSystems 1:026f79cfd378 169 controls.channel_7 = convert_pulsewidth(rc_channel_7.pulsewidth());
IonSystems 1:026f79cfd378 170 controls.channel_8 = convert_pulsewidth(rc_channel_8.pulsewidth());
IonSystems 1:026f79cfd378 171
IonSystems 1:026f79cfd378 172 /* Check if rc is stalled and failsafe output */
IonSystems 1:026f79cfd378 173
IonSystems 1:026f79cfd378 174 if(rc_channel_1.stallTimer.read_ms() > 200){
IonSystems 1:026f79cfd378 175 failsafe = true;
IonSystems 1:026f79cfd378 176 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 177 pc.printf("NO SIGNAL - FAILSAFE ACTIVATED \r\n");
IonSystems 1:026f79cfd378 178 #endif
IonSystems 1:026f79cfd378 179 failsafe_timer++;
IonSystems 1:026f79cfd378 180 wait_ms(100);
IonSystems 1:026f79cfd378 181 if(failsafe_timer > NO_SIGNAL_TIMEOUT){
IonSystems 1:026f79cfd378 182 armed = false;
IonSystems 1:026f79cfd378 183 }
IonSystems 1:026f79cfd378 184 }else{
IonSystems 1:026f79cfd378 185 failsafe_timer = 0;
IonSystems 1:026f79cfd378 186 failsafe = false;
IonSystems 1:026f79cfd378 187 }
IonSystems 1:026f79cfd378 188
IonSystems 1:026f79cfd378 189
IonSystems 1:026f79cfd378 190
IonSystems 1:026f79cfd378 191 direction.rotation = controls.channel_4;
IonSystems 1:026f79cfd378 192 direction.x_translation = controls.channel_2;
IonSystems 1:026f79cfd378 193 direction.y_translation = controls.channel_1;
IonSystems 1:026f79cfd378 194
IonSystems 1:026f79cfd378 195 /* Heading lock enable switch */
IonSystems 1:026f79cfd378 196 if(controls.channel_8 > 75 && !heading_lock_enabled){
IonSystems 1:026f79cfd378 197 heading_lock = orientation.heading;
IonSystems 1:026f79cfd378 198 heading_lock_enabled = true;
IonSystems 1:026f79cfd378 199 }else if(controls.channel_8 < 75 && heading_lock_enabled){
IonSystems 1:026f79cfd378 200 heading_lock_enabled = false;
IonSystems 1:026f79cfd378 201 }
IonSystems 1:026f79cfd378 202
IonSystems 1:026f79cfd378 203 /* Speed for heading lock */
IonSystems 1:026f79cfd378 204 heading_lock_speed = (int) controls.channel_7;
IonSystems 1:026f79cfd378 205
IonSystems 1:026f79cfd378 206 #if defined (PC_DEBUGGING) && defined (DEBUG_INPUT)
IonSystems 1:026f79cfd378 207 pc.printf("Input: C1: %i \t C2: %i \t C3: %i \t C4: %i \t C5: %i \t C6: %i \t C7: %i \t 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 1:026f79cfd378 208
IonSystems 1:026f79cfd378 209 pc.printf("Heading Lock: %s \r\n", heading_lock_enabled ? "true" : "false");
IonSystems 1:026f79cfd378 210 pc.printf("Heading Lock Speed: %i \r\n", heading_lock_speed);
IonSystems 1:026f79cfd378 211 #endif
IonSystems 1:026f79cfd378 212 }
IonSystems 1:026f79cfd378 213 }
IonSystems 1:026f79cfd378 214
IonSystems 1:026f79cfd378 215
IonSystems 0:d6eeeae3c3cb 216 /** Calculate Orientation
IonSystems 0:d6eeeae3c3cb 217 *
IonSystems 0:d6eeeae3c3cb 218 * Calculates orientation using the BNO055 sensor
IonSystems 0:d6eeeae3c3cb 219 */
IonSystems 0:d6eeeae3c3cb 220 void calculate_orientation(){
IonSystems 0:d6eeeae3c3cb 221
IonSystems 0:d6eeeae3c3cb 222 /* If there is an error then we maintain the same
IonSystems 0:d6eeeae3c3cb 223 * orientation to stop random control flipping */
IonSystems 0:d6eeeae3c3cb 224 if(!bno055Healthy()){
IonSystems 1:026f79cfd378 225 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 0:d6eeeae3c3cb 226 pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
IonSystems 1:026f79cfd378 227 #endif
IonSystems 0:d6eeeae3c3cb 228 }else{
IonSystems 0:d6eeeae3c3cb 229
IonSystems 0:d6eeeae3c3cb 230 /* Read in the Euler angles */
IonSystems 0:d6eeeae3c3cb 231 orientation = getEulerAngles();
IonSystems 0:d6eeeae3c3cb 232
IonSystems 1:026f79cfd378 233 /* We are upside down in range -30 -> -90
IonSystems 1:026f79cfd378 234 * the sensor will report -60 when inverted */
IonSystems 0:d6eeeae3c3cb 235 if(orientation.roll < -30 && orientation.roll > -90){
IonSystems 0:d6eeeae3c3cb 236 inverted = true;
IonSystems 0:d6eeeae3c3cb 237 }else{
IonSystems 0:d6eeeae3c3cb 238 inverted = false;
IonSystems 0:d6eeeae3c3cb 239 }
IonSystems 0:d6eeeae3c3cb 240 #if defined (PC_DEBUGGING) && defined (DEBUG_ORIENTATION)
IonSystems 1:026f79cfd378 241 pc.printf("Inverted= %s \t (%7.2f) \r\n", inverted ? "true" : "false", orientation.roll);
IonSystems 0:d6eeeae3c3cb 242 #endif
IonSystems 0:d6eeeae3c3cb 243 }
IonSystems 0:d6eeeae3c3cb 244
IonSystems 0:d6eeeae3c3cb 245
IonSystems 0:d6eeeae3c3cb 246 }
IonSystems 0:d6eeeae3c3cb 247
IonSystems 0:d6eeeae3c3cb 248 /** Calculate controls
IonSystems 0:d6eeeae3c3cb 249 *
IonSystems 0:d6eeeae3c3cb 250 * Calculates controls based on if the robot is inverted or not
IonSystems 1:026f79cfd378 251 * TODO: Add inverted support
IonSystems 0:d6eeeae3c3cb 252 */
IonSystems 0:d6eeeae3c3cb 253 void calculate_controls(){
IonSystems 1:026f79cfd378 254
IonSystems 1:026f79cfd378 255 #ifdef OMNI_MIXER
IonSystems 0:d6eeeae3c3cb 256
IonSystems 1:026f79cfd378 257 float x = direction.x_translation -50;
IonSystems 1:026f79cfd378 258 float y = direction.y_translation -50;
IonSystems 0:d6eeeae3c3cb 259
IonSystems 1:026f79cfd378 260 float theta = (float)atan2((double)x, (double)y);
IonSystems 1:026f79cfd378 261 float magnitude = (float)sqrt((double)((x*x)+(y*y)));
IonSystems 0:d6eeeae3c3cb 262
IonSystems 1:026f79cfd378 263 if(magnitude > 5.0f){
IonSystems 0:d6eeeae3c3cb 264
IonSystems 1:026f79cfd378 265 float vx = magnitude * sin(theta);
IonSystems 1:026f79cfd378 266 float vy = magnitude * cos(theta);
IonSystems 1:026f79cfd378 267 const float sqrt3o2 = 1.0*sqrt(3.0)/2.0;
IonSystems 0:d6eeeae3c3cb 268
IonSystems 1:026f79cfd378 269 float w0 = -vx; // v dot [-1, 0] / 25mm
IonSystems 1:026f79cfd378 270 float w1 = 0.5*vx - sqrt3o2 * vy; // v dot [1/2, -sqrt(3)/2] / 25mm
IonSystems 1:026f79cfd378 271 float w2 = 0.5*vx + sqrt3o2 * vy; // v dot [1/2, +sqrt(3)/2] / 25mm
IonSystems 1:026f79cfd378 272 #if defined (PC_DEBUGGING) && defined (DEBUG_CONTROLS)
IonSystems 1:026f79cfd378 273 pc.printf("Calculated Controls: (%7.2f) \t (%7.2f) \t (%7.2f) \r\n", w0, w1, w2);
IonSystems 1:026f79cfd378 274 #endif
IonSystems 1:026f79cfd378 275 float w0_speed = map(w0, -70, 70, 0, 100);
IonSystems 1:026f79cfd378 276 float w1_speed = map(w1, -70, 70, 0, 100);
IonSystems 1:026f79cfd378 277 float w2_speed = map(w2, -70, 70, 0, 100);
IonSystems 1:026f79cfd378 278
IonSystems 1:026f79cfd378 279 /* Add in rotation */
IonSystems 1:026f79cfd378 280 #if defined (PC_DEBUGGING) && defined (DEBUG_CONTROLS)
IonSystems 1:026f79cfd378 281 pc.printf("Mapped Controls: (%7.2f) \t (%7.2f) \t (%7.2f) \r\n", w0_speed, w1_speed, w2_speed);
IonSystems 1:026f79cfd378 282 #endif
IonSystems 1:026f79cfd378 283 outputs.wheel_1 += w0_speed -50;
IonSystems 1:026f79cfd378 284 outputs.wheel_2 += w1_speed -50;
IonSystems 1:026f79cfd378 285 outputs.wheel_3 += w2_speed -50;
IonSystems 1:026f79cfd378 286
IonSystems 1:026f79cfd378 287
IonSystems 1:026f79cfd378 288 }else{
IonSystems 1:026f79cfd378 289 outputs.wheel_1 = 50;
IonSystems 1:026f79cfd378 290 outputs.wheel_2 = 50;
IonSystems 1:026f79cfd378 291 outputs.wheel_3 = 50;
IonSystems 1:026f79cfd378 292 }
IonSystems 0:d6eeeae3c3cb 293
IonSystems 1:026f79cfd378 294 outputs.wheel_1 += direction.rotation -50;
IonSystems 1:026f79cfd378 295 outputs.wheel_2 += direction.rotation -50;
IonSystems 1:026f79cfd378 296 outputs.wheel_3 += direction.rotation -50;
IonSystems 1:026f79cfd378 297
IonSystems 1:026f79cfd378 298 #else
IonSystems 1:026f79cfd378 299
IonSystems 1:026f79cfd378 300 outputs.wheel_1 = direction.rotation;
IonSystems 1:026f79cfd378 301 outputs.wheel_2 = direction.x_translation;
IonSystems 1:026f79cfd378 302 outputs.wheel_3 = direction.y_translation;
IonSystems 1:026f79cfd378 303
IonSystems 1:026f79cfd378 304 #endif
IonSystems 1:026f79cfd378 305
IonSystems 1:026f79cfd378 306 /* Clamp outputs to correct range */
IonSystems 1:026f79cfd378 307 outputs.wheel_1 = clamp(outputs.wheel_1, 0, 100);
IonSystems 1:026f79cfd378 308 outputs.wheel_2 = clamp(outputs.wheel_2, 0, 100);
IonSystems 1:026f79cfd378 309 outputs.wheel_3 = clamp(outputs.wheel_3, 0, 100);
IonSystems 1:026f79cfd378 310
IonSystems 1:026f79cfd378 311 #if defined (PC_DEBUGGING) && defined (DEBUG_CONTROLS)
IonSystems 1:026f79cfd378 312 pc.printf("Final Controls: %i \t %i \t %i \r\n", outputs.wheel_1, outputs.wheel_2, outputs.wheel_3);
IonSystems 1:026f79cfd378 313 #endif
IonSystems 1:026f79cfd378 314
IonSystems 0:d6eeeae3c3cb 315 }
IonSystems 0:d6eeeae3c3cb 316
IonSystems 0:d6eeeae3c3cb 317 /** Heading correction
IonSystems 0:d6eeeae3c3cb 318 *
IonSystems 0:d6eeeae3c3cb 319 * Works out which way to spin to return to set heading
IonSystems 0:d6eeeae3c3cb 320 * Based on tiberius turnto code
IonSystems 0:d6eeeae3c3cb 321 *
IonSystems 0:d6eeeae3c3cb 322 * @param speed speed to rotate in percent
IonSystems 0:d6eeeae3c3cb 323 */
IonSystems 1:026f79cfd378 324 void calculate_heading_correction(){
IonSystems 1:026f79cfd378 325 if(heading_lock_enabled){
IonSystems 1:026f79cfd378 326
IonSystems 1:026f79cfd378 327 /* Normalize headings */
IonSystems 1:026f79cfd378 328 float n_current_heading = normalize(orientation.heading);
IonSystems 1:026f79cfd378 329 float n_heading_lock = normalize(heading_lock);
IonSystems 1:026f79cfd378 330
IonSystems 1:026f79cfd378 331 #if defined (PC_DEBUGGING) && defined (DEBUG_HEADING_LOCK)
IonSystems 1:026f79cfd378 332 pc.printf("Current heading: %f \t Desired Heading: %f \r\n", n_current_heading, n_heading_lock);
IonSystems 1:026f79cfd378 333 #endif
IonSystems 1:026f79cfd378 334
IonSystems 1:026f79cfd378 335 /* Calculate error */
IonSystems 1:026f79cfd378 336 float error = n_current_heading - n_heading_lock;
IonSystems 1:026f79cfd378 337
IonSystems 1:026f79cfd378 338 /* Normalize error */
IonSystems 1:026f79cfd378 339 error = normalize(error);
IonSystems 0:d6eeeae3c3cb 340
IonSystems 1:026f79cfd378 341 /* Figure out which way to turn */
IonSystems 1:026f79cfd378 342 if(abs(error) > heading_lock_deadband){
IonSystems 1:026f79cfd378 343
IonSystems 1:026f79cfd378 344 /* Speed is 0 -> 100
IonSystems 1:026f79cfd378 345 * error is -180 -> 180
IonSystems 1:026f79cfd378 346 * output is 0 -> 100
IonSystems 1:026f79cfd378 347 */
IonSystems 1:026f79cfd378 348 //error = error * (heading_lock_speed / 100.0);
IonSystems 1:026f79cfd378 349 #if defined (PC_DEBUGGING) && defined (DEBUG_HEADING_LOCK)
IonSystems 1:026f79cfd378 350 pc.printf("ERROR: %7.2f", error);
IonSystems 1:026f79cfd378 351 #endif
IonSystems 1:026f79cfd378 352 error = map(error, -180, 180, 1.0, -1.0);
IonSystems 1:026f79cfd378 353
IonSystems 1:026f79cfd378 354
IonSystems 1:026f79cfd378 355 float amount = error * 50;
IonSystems 1:026f79cfd378 356
IonSystems 1:026f79cfd378 357 amount *= heading_lock_speed / 100.0;
IonSystems 1:026f79cfd378 358
IonSystems 1:026f79cfd378 359 direction.rotation += amount;
IonSystems 1:026f79cfd378 360
IonSystems 1:026f79cfd378 361 }else{
IonSystems 1:026f79cfd378 362
IonSystems 1:026f79cfd378 363 /* In deadband so do nothing */
IonSystems 1:026f79cfd378 364 direction.rotation = 50;
IonSystems 1:026f79cfd378 365 }
IonSystems 1:026f79cfd378 366 #if defined (PC_DEBUGGING) && defined (DEBUG_HEADING_LOCK)
IonSystems 1:026f79cfd378 367 pc.printf("Heading error: (%7.2f) \t Rotation: (%7.2f) \t Speed: (%7.2f) \r\n", error, direction.rotation, heading_lock_speed);
IonSystems 1:026f79cfd378 368 #endif
IonSystems 1:026f79cfd378 369
IonSystems 1:026f79cfd378 370 }
IonSystems 1:026f79cfd378 371 }
IonSystems 1:026f79cfd378 372
IonSystems 1:026f79cfd378 373 /** Output to OmniMixer and ESCs
IonSystems 1:026f79cfd378 374 *
IonSystems 1:026f79cfd378 375 * Sends calculated output values to devices
IonSystems 1:026f79cfd378 376 */
IonSystems 1:026f79cfd378 377 void output_stage(){
IonSystems 1:026f79cfd378 378 if(!failsafe){
IonSystems 1:026f79cfd378 379 //outputs.wheel_1 -= 18;
IonSystems 1:026f79cfd378 380 //outputs.wheel_2 -= 27;
IonSystems 1:026f79cfd378 381 //outputs.wheel_3 -= 14;
IonSystems 0:d6eeeae3c3cb 382
IonSystems 1:026f79cfd378 383 esc_omni_1.setThrottle(outputs.wheel_1);
IonSystems 1:026f79cfd378 384 esc_omni_2.setThrottle(outputs.wheel_2);
IonSystems 1:026f79cfd378 385 esc_omni_3.setThrottle(outputs.wheel_3);
IonSystems 1:026f79cfd378 386 //esc_weapon_1.setThrottle(outputs.weapon_motor_1);
IonSystems 1:026f79cfd378 387 //esc_weapon_2.setThrottle(outputs.weapon_motor_2);
IonSystems 1:026f79cfd378 388 }else{
IonSystems 1:026f79cfd378 389 esc_omni_1.failsafe();
IonSystems 1:026f79cfd378 390 esc_omni_2.failsafe();
IonSystems 1:026f79cfd378 391 esc_omni_3.failsafe();
IonSystems 0:d6eeeae3c3cb 392 }
IonSystems 0:d6eeeae3c3cb 393
IonSystems 1:026f79cfd378 394 #if defined (PC_DEBUGGING) && defined (DEBUG_OUTPUT)
IonSystems 1:026f79cfd378 395 pc.printf("OUTPUT M1: %i \t M2: %i \t M3: %i \t W1: %i \t W2: %i \r\n", outputs.wheel_1, outputs.wheel_2, outputs.wheel_3, outputs.weapon_motor_1, outputs.weapon_motor_2);
IonSystems 1:026f79cfd378 396 #endif
IonSystems 0:d6eeeae3c3cb 397 }
IonSystems 0:d6eeeae3c3cb 398
IonSystems 1:026f79cfd378 399 /******************************************************************************/
IonSystems 1:026f79cfd378 400
IonSystems 0:d6eeeae3c3cb 401 /** Main
IonSystems 0:d6eeeae3c3cb 402 *
IonSystems 0:d6eeeae3c3cb 403 * Main Loop
IonSystems 0:d6eeeae3c3cb 404 */
IonSystems 0:d6eeeae3c3cb 405 int main() {
IonSystems 0:d6eeeae3c3cb 406 #ifdef PC_DEBUGGING
IonSystems 0:d6eeeae3c3cb 407 pc.baud(115200);
IonSystems 0:d6eeeae3c3cb 408 pc.printf("Triforce Control System \r\n");
IonSystems 0:d6eeeae3c3cb 409 #endif
IonSystems 0:d6eeeae3c3cb 410
IonSystems 0:d6eeeae3c3cb 411 init();
IonSystems 0:d6eeeae3c3cb 412
IonSystems 0:d6eeeae3c3cb 413 while(true){
IonSystems 1:026f79cfd378 414 led1 = armed;
IonSystems 1:026f79cfd378 415 reset_outputs();
IonSystems 0:d6eeeae3c3cb 416
IonSystems 1:026f79cfd378 417 #ifdef PC_DEBUGGING
IonSystems 1:026f79cfd378 418 pc.printf("\x1b[2J\x1b[H");
IonSystems 0:d6eeeae3c3cb 419 #endif
IonSystems 0:d6eeeae3c3cb 420
IonSystems 1:026f79cfd378 421 read_inputs();
IonSystems 1:026f79cfd378 422 if(armed && !just_armed){
IonSystems 1:026f79cfd378 423 led1 = armed;
IonSystems 1:026f79cfd378 424 #ifdef ORIENTATION
IonSystems 1:026f79cfd378 425 calculate_orientation();
IonSystems 1:026f79cfd378 426 #endif
IonSystems 1:026f79cfd378 427
IonSystems 1:026f79cfd378 428 #ifdef HEADING_LOCK
IonSystems 1:026f79cfd378 429 calculate_heading_correction();
IonSystems 1:026f79cfd378 430 #endif
IonSystems 1:026f79cfd378 431 calculate_controls();
IonSystems 1:026f79cfd378 432
IonSystems 1:026f79cfd378 433 output_stage();
IonSystems 1:026f79cfd378 434 }
IonSystems 1:026f79cfd378 435 #ifdef PC_DEBUGGING
IonSystems 1:026f79cfd378 436 wait_ms(50);
IonSystems 1:026f79cfd378 437 #endif
IonSystems 1:026f79cfd378 438 }
IonSystems 0:d6eeeae3c3cb 439 }
IonSystems 0:d6eeeae3c3cb 440
IonSystems 0:d6eeeae3c3cb 441