Module 9 Super Team / Mbed 2 deprecated hookey_controller

The total controller!

Dependencies:   EMG1 PIDController1 compute mbed InBetweenController PinDetect QEI

main.cpp@22:c6459c435069, 2015-11-05 (annotated)

Committer:
dolcaer
Date:
Thu Nov 05 10:47:06 2015 +0000
Revision:
22:c6459c435069
Parent:
21:9de9e0a73e57
Child:
23:174f955e5c15
Adds demo mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dolcaer 0:5f9d8f2142b6 1 #include "mbed.h"
dolcaer 0:5f9d8f2142b6 2 #include "emg.h"
AeroKev 3:0634f55c2021 3 #include "inbetweencontroller.h"
AeroKev 3:0634f55c2021 4 #include "pidControl.h"
AeroKev 5:5339a7be9fb7 5 #include "PinDetect.h"
AeroKev 7:aad7d5cdecd3 6 #include "compute.h"
dolcaer 22:c6459c435069 7
AeroKev 21:9de9e0a73e57 8 /// Define Objects
AeroKev 3:0634f55c2021 9 Ticker sample_timer;
AeroKev 7:aad7d5cdecd3 10 Ticker movement_timer;
AeroKev 8:7814ba9f801e 11 Ticker calc_timer;
AeroKev 14:0a66bce6ee19 12 PinDetect sRound(SW2);
AeroKev 14:0a66bce6ee19 13 PinDetect caliAndSS(SW3);
AeroKev 7:aad7d5cdecd3 14 bool started = true;
AeroKev 7:aad7d5cdecd3 15 bool go_motor = true;
AeroKev 7:aad7d5cdecd3 16 bool go_tick = false;
AeroKev 8:7814ba9f801e 17 bool go_sample = false;
dolcaer 11:46a859e526ca 18 double motorSpeed = 0.2;
AeroKev 14:0a66bce6ee19 19 int calibrated = 0;
AeroKev 8:7814ba9f801e 20 double emg_out0, emg_out1, emg_out2;
dolcaer 22:c6459c435069 21 bool demoing = false;
dolcaer 22:c6459c435069 22 int demoInt = 0;
dolcaer 22:c6459c435069 23
AeroKev 7:aad7d5cdecd3 24 void tick()
AeroKev 7:aad7d5cdecd3 25 {
dolcaer 22:c6459c435069 26 if(demoing) {
dolcaer 22:c6459c435069 27 double a,b;
dolcaer 22:c6459c435069 28 if(demoInt == 0)
dolcaer 22:c6459c435069 29 Point2Angles(-20, 18, a, b);
dolcaer 22:c6459c435069 30 if(demoInt == 1)
dolcaer 22:c6459c435069 31 Point2Angles(-20, 35, a, b);
dolcaer 22:c6459c435069 32 if(demoInt == 2)
dolcaer 22:c6459c435069 33 Point2Angles(20, 35, a, b);
dolcaer 22:c6459c435069 34 if(demoInt == 3)
dolcaer 22:c6459c435069 35 Point2Angles(20, 18, a, b);
dolcaer 22:c6459c435069 36 demoInt++;
dolcaer 22:c6459c435069 37 demoInt = demoInt%4;
dolcaer 22:c6459c435069 38 rotate(a,b);
dolcaer 22:c6459c435069 39 return;
dolcaer 22:c6459c435069 40 }
AeroKev 9:70cab4bf38a5 41 double a,b,x,y;
dolcaer 22:c6459c435069 42
AeroKev 21:9de9e0a73e57 43 /** If the game has not started, don't do anything. */
AeroKev 5:5339a7be9fb7 44 if(!started) return;
dolcaer 22:c6459c435069 45
AeroKev 21:9de9e0a73e57 46 /// Get the current rotation
AeroKev 9:70cab4bf38a5 47 getCurrent(a,b);
dolcaer 22:c6459c435069 48
AeroKev 21:9de9e0a73e57 49 /// Remove the offset
AeroKev 8:7814ba9f801e 50 a = getOffset(1) - a;
AeroKev 8:7814ba9f801e 51 b = getOffset(2) - b;
dolcaer 22:c6459c435069 52
AeroKev 21:9de9e0a73e57 53 /// Calculate the new position using the EMG output and the current position
dolcaer 11:46a859e526ca 54 bool pushing;
dolcaer 11:46a859e526ca 55 newPos(emg_out0, emg_out1, emg_out2, a, b, x, y, pushing);
dolcaer 22:c6459c435069 56
dolcaer 22:c6459c435069 57 if (pushing)
dolcaer 11:46a859e526ca 58 motorSpeed = 1;
dubbie 16:b977ba7ed362 59 else if(a < -20 || a > 20)
dolcaer 11:46a859e526ca 60 motorSpeed = 0.14;
AeroKev 10:3cee5adfbd72 61 else
dolcaer 11:46a859e526ca 62 motorSpeed = 0.25;
dolcaer 22:c6459c435069 63
AeroKev 21:9de9e0a73e57 64 /// Rotate the motors
dolcaer 11:46a859e526ca 65 if (pushing)
dolcaer 11:46a859e526ca 66 push(x, y);
dolcaer 11:46a859e526ca 67 else
dolcaer 11:46a859e526ca 68 rotate(x,y);
dolcaer 22:c6459c435069 69
dolcaer 0:5f9d8f2142b6 70 }
dolcaer 22:c6459c435069 71
AeroKev 21:9de9e0a73e57 72 /** Functions which get called by tickers */
dubbie 16:b977ba7ed362 73 void goMotor()
dubbie 16:b977ba7ed362 74 {
dubbie 16:b977ba7ed362 75 go_motor = true;
dubbie 16:b977ba7ed362 76 }
dolcaer 22:c6459c435069 77
dubbie 16:b977ba7ed362 78 void goTick()
dubbie 16:b977ba7ed362 79 {
dubbie 16:b977ba7ed362 80 go_tick = true;
dubbie 16:b977ba7ed362 81 }
dolcaer 22:c6459c435069 82
dubbie 16:b977ba7ed362 83 void goSample()
dubbie 16:b977ba7ed362 84 {
dubbie 16:b977ba7ed362 85 go_sample = true;
dubbie 16:b977ba7ed362 86 }
dolcaer 22:c6459c435069 87
AeroKev 21:9de9e0a73e57 88 /** Function called by pressing the SW3 Button */
AeroKev 14:0a66bce6ee19 89 void calibrateAndStartStop()
AeroKev 7:aad7d5cdecd3 90 {
dubbie 16:b977ba7ed362 91 if(calibrated < 4) {
dubbie 16:b977ba7ed362 92 emg_cal(calibrated);
AeroKev 14:0a66bce6ee19 93 calibrated++;
dubbie 16:b977ba7ed362 94 return;
AeroKev 14:0a66bce6ee19 95 }
AeroKev 5:5339a7be9fb7 96 started = !started;
AeroKev 18:ff312179cf44 97 if(!started) {
AeroKev 7:aad7d5cdecd3 98 go_motor = false;
AeroKev 7:aad7d5cdecd3 99 PID_stop();
AeroKev 7:aad7d5cdecd3 100 movement_timer.detach();
dolcaer 22:c6459c435069 101 } else
dubbie 16:b977ba7ed362 102 movement_timer.attach(&goMotor, 0.01f);
AeroKev 5:5339a7be9fb7 103 }
dolcaer 22:c6459c435069 104
AeroKev 21:9de9e0a73e57 105 /** Function called by pressing the SW2 Button */
AeroKev 14:0a66bce6ee19 106 void startRound()
AeroKev 7:aad7d5cdecd3 107 {
dolcaer 22:c6459c435069 108 if(calibrated < 3) {
dolcaer 22:c6459c435069 109 calibrated = 4;
dolcaer 22:c6459c435069 110 calc_timer.attach(&goTick, 2.0);
dolcaer 22:c6459c435069 111 demoing = true;
dolcaer 22:c6459c435069 112 }
dolcaer 22:c6459c435069 113 else {
dolcaer 22:c6459c435069 114 sample_timer.attach(&goSample, 0.002);
dolcaer 22:c6459c435069 115 calc_timer.attach(&goTick, 0.15);
dolcaer 22:c6459c435069 116 }
AeroKev 5:5339a7be9fb7 117 }
dolcaer 22:c6459c435069 118
AeroKev 21:9de9e0a73e57 119 /** Initialize the buttons */
AeroKev 7:aad7d5cdecd3 120 void init()
AeroKev 7:aad7d5cdecd3 121 {
AeroKev 21:9de9e0a73e57 122 /// Set the shutup and start buttons
AeroKev 14:0a66bce6ee19 123 caliAndSS.mode(PullUp);
AeroKev 14:0a66bce6ee19 124 caliAndSS.attach_deasserted(&calibrateAndStartStop);
AeroKev 14:0a66bce6ee19 125 caliAndSS.setSampleFrequency();
dolcaer 22:c6459c435069 126
AeroKev 14:0a66bce6ee19 127 sRound.mode(PullUp);
AeroKev 14:0a66bce6ee19 128 sRound.attach_deasserted(&startRound);
AeroKev 14:0a66bce6ee19 129 sRound.setSampleFrequency();
AeroKev 5:5339a7be9fb7 130 }
dolcaer 22:c6459c435069 131
AeroKev 21:9de9e0a73e57 132 /** The main function
AeroKev 9:70cab4bf38a5 133 Initializes every library
AeroKev 9:70cab4bf38a5 134 Starts the timers
AeroKev 9:70cab4bf38a5 135 Has a while-loop which calls functions on a timer
AeroKev 9:70cab4bf38a5 136 */
dolcaer 0:5f9d8f2142b6 137 int main()
dolcaer 0:5f9d8f2142b6 138 {
AeroKev 21:9de9e0a73e57 139 /** Initialize libraries */
AeroKev 6:4f8760baa448 140 double a,b;
AeroKev 6:4f8760baa448 141 IBC_init(a,b);
AeroKev 9:70cab4bf38a5 142 PID_init();
dolcaer 22:c6459c435069 143
AeroKev 21:9de9e0a73e57 144 /** Enable buttons */
dubbie 16:b977ba7ed362 145 init();
dolcaer 22:c6459c435069 146
AeroKev 7:aad7d5cdecd3 147 movement_timer.attach(&goMotor, 0.01f);
dolcaer 22:c6459c435069 148
dubbie 17:b1d8c215b7ea 149 rotate(a,b);
dolcaer 22:c6459c435069 150
AeroKev 21:9de9e0a73e57 151 /** The main while-loop */
AeroKev 7:aad7d5cdecd3 152 while(1) {
AeroKev 21:9de9e0a73e57 153 /// The motor timer
AeroKev 7:aad7d5cdecd3 154 if(go_motor) {
AeroKev 7:aad7d5cdecd3 155 go_motor = false;
AeroKev 10:3cee5adfbd72 156 moveTick(motorSpeed);
AeroKev 7:aad7d5cdecd3 157 }
AeroKev 21:9de9e0a73e57 158 /// The sample timer
AeroKev 8:7814ba9f801e 159 if(go_sample) {
AeroKev 8:7814ba9f801e 160 go_sample = false;
dubbie 16:b977ba7ed362 161 sample(emg_out0, emg_out1, emg_out2);
AeroKev 8:7814ba9f801e 162 }
AeroKev 21:9de9e0a73e57 163 /// The tick timer
AeroKev 7:aad7d5cdecd3 164 if(go_tick) {
AeroKev 7:aad7d5cdecd3 165 go_tick = false;
AeroKev 7:aad7d5cdecd3 166 tick();
AeroKev 7:aad7d5cdecd3 167 }
AeroKev 7:aad7d5cdecd3 168 }
dolcaer 22:c6459c435069 169 }