Module 9 Super Team / Mbed 2 deprecated hookey_controller

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EMG1 PIDController1 compute mbed InBetweenController PinDetect QEI

main.cpp@11:46a859e526ca, 2015-10-28 (annotated)

Committer:
dolcaer
Date:
Wed Oct 28 13:40:35 2015 +0000
Revision:
11:46a859e526ca
Parent:
10:3cee5adfbd72
Child:
14:0a66bce6ee19
Child:
15:59d7fffd070a
Improved left-to-right control, but pushing oscillates heavily. Needs to be tested with EMG. NOTE: Still contains debug code to work without EMG.

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"
AeroKev 3:0634f55c2021 7
AeroKev 3:0634f55c2021 8 // Define Objects
AeroKev 5:5339a7be9fb7 9 Serial pc2(USBTX, USBRX);
AeroKev 3:0634f55c2021 10 Ticker sample_timer;
AeroKev 7:aad7d5cdecd3 11 Ticker movement_timer;
AeroKev 8:7814ba9f801e 12 Ticker calc_timer;
AeroKev 5:5339a7be9fb7 13 PinDetect cali(SW2);
AeroKev 5:5339a7be9fb7 14 PinDetect start(SW3);
AeroKev 7:aad7d5cdecd3 15 bool started = true;
AeroKev 7:aad7d5cdecd3 16 bool go_motor = true;
AeroKev 7:aad7d5cdecd3 17 bool go_tick = false;
AeroKev 8:7814ba9f801e 18 bool go_sample = false;
dolcaer 11:46a859e526ca 19 double motorSpeed = 0.2;
AeroKev 8:7814ba9f801e 20
AeroKev 8:7814ba9f801e 21 double emg_out0, emg_out1, emg_out2;
dolcaer 0:5f9d8f2142b6 22
dolcaer 11:46a859e526ca 23 int debug_tick = 0;
AeroKev 7:aad7d5cdecd3 24 void tick()
AeroKev 7:aad7d5cdecd3 25 {
AeroKev 9:70cab4bf38a5 26 double a,b,x,y;
AeroKev 9:70cab4bf38a5 27
AeroKev 9:70cab4bf38a5 28 /* If the game has not started, don't do anything. */
AeroKev 5:5339a7be9fb7 29 if(!started) return;
AeroKev 8:7814ba9f801e 30
AeroKev 9:70cab4bf38a5 31 // Get the current rotation
AeroKev 9:70cab4bf38a5 32 getCurrent(a,b);
AeroKev 8:7814ba9f801e 33
AeroKev 9:70cab4bf38a5 34 // Remove the offset
AeroKev 8:7814ba9f801e 35 a = getOffset(1) - a;
AeroKev 8:7814ba9f801e 36 b = getOffset(2) - b;
dolcaer 11:46a859e526ca 37
dolcaer 11:46a859e526ca 38 // TODO: Remove debugging overrides
dolcaer 11:46a859e526ca 39 if (debug_tick < 5) {
dolcaer 11:46a859e526ca 40 emg_out0 = 0;
dolcaer 11:46a859e526ca 41 emg_out1 = 0;
dolcaer 11:46a859e526ca 42 emg_out2 = 0;
dolcaer 11:46a859e526ca 43 } else if (debug_tick < 10) {
dolcaer 11:46a859e526ca 44 emg_out0 = 0;
dolcaer 11:46a859e526ca 45 emg_out1 = 0;
dolcaer 11:46a859e526ca 46 emg_out2 = 1;
dolcaer 11:46a859e526ca 47 } else if (debug_tick < 20) {
dolcaer 11:46a859e526ca 48 emg_out0 = 0;
dolcaer 11:46a859e526ca 49 emg_out1 = 0;
dolcaer 11:46a859e526ca 50 emg_out2 = 0;
dolcaer 11:46a859e526ca 51 } else if (debug_tick < 25) {
dolcaer 11:46a859e526ca 52 emg_out0 = 1;
dolcaer 11:46a859e526ca 53 emg_out1 = 0;
dolcaer 11:46a859e526ca 54 emg_out2 = 0;
dolcaer 11:46a859e526ca 55 } else if (debug_tick < 30) {
dolcaer 11:46a859e526ca 56 emg_out0 = 0;
dolcaer 11:46a859e526ca 57 emg_out1 = 0;
dolcaer 11:46a859e526ca 58 emg_out2 = 0;
dolcaer 11:46a859e526ca 59 } else if (debug_tick < 40) {
dolcaer 11:46a859e526ca 60 emg_out0 = 0;
dolcaer 11:46a859e526ca 61 emg_out1 = 0;
dolcaer 11:46a859e526ca 62 emg_out2 = 1;
dolcaer 11:46a859e526ca 63 } else if (debug_tick < 45) {
dolcaer 11:46a859e526ca 64 emg_out0 = 0;
dolcaer 11:46a859e526ca 65 emg_out1 = 0;
dolcaer 11:46a859e526ca 66 emg_out2 = 0;
dolcaer 11:46a859e526ca 67 } else if (debug_tick < 55) {
dolcaer 11:46a859e526ca 68 emg_out0 = 0;
dolcaer 11:46a859e526ca 69 emg_out1 = 1;
dolcaer 11:46a859e526ca 70 emg_out2 = 0;
dolcaer 11:46a859e526ca 71 } else if (debug_tick < 60) {
dolcaer 11:46a859e526ca 72 emg_out0 = 0;
dolcaer 11:46a859e526ca 73 emg_out1 = 0;
dolcaer 11:46a859e526ca 74 emg_out2 = 0;
dolcaer 11:46a859e526ca 75 } else if (debug_tick < 70) {
dolcaer 11:46a859e526ca 76 emg_out0 = 0;
dolcaer 11:46a859e526ca 77 emg_out1 = 0;
dolcaer 11:46a859e526ca 78 emg_out2 = 1;
dolcaer 11:46a859e526ca 79 } else {
dolcaer 11:46a859e526ca 80 emg_out0 = 0;
dolcaer 11:46a859e526ca 81 emg_out1 = 0;
dolcaer 11:46a859e526ca 82 emg_out2 = 0;
dolcaer 11:46a859e526ca 83 }
dolcaer 11:46a859e526ca 84
dolcaer 11:46a859e526ca 85 /*if (debug_tick < 10) {
dolcaer 11:46a859e526ca 86 emg_out0 = 0;
dolcaer 11:46a859e526ca 87 emg_out1 = 0;
dolcaer 11:46a859e526ca 88 emg_out2 = 0;
dolcaer 11:46a859e526ca 89 } else if (debug_tick < 15) {
dolcaer 11:46a859e526ca 90 emg_out0 = 1;
dolcaer 11:46a859e526ca 91 emg_out1 = 0;
dolcaer 11:46a859e526ca 92 emg_out2 = 0;
dolcaer 11:46a859e526ca 93 } else if (debug_tick < 25) {
dolcaer 11:46a859e526ca 94 emg_out0 = 0;
dolcaer 11:46a859e526ca 95 emg_out1 = 1;
dolcaer 11:46a859e526ca 96 emg_out2 = 0;
dolcaer 11:46a859e526ca 97 } else if (debug_tick < 35) {
dolcaer 11:46a859e526ca 98 emg_out0 = 0;
dolcaer 11:46a859e526ca 99 emg_out1 = 0;
dolcaer 11:46a859e526ca 100 emg_out2 = 0;
dolcaer 11:46a859e526ca 101 } else if (debug_tick < 45) {
dolcaer 11:46a859e526ca 102 emg_out0 = 1;
dolcaer 11:46a859e526ca 103 emg_out1 = 0;
dolcaer 11:46a859e526ca 104 emg_out2 = 0;
dolcaer 11:46a859e526ca 105 } else if (debug_tick < 55) {
dolcaer 11:46a859e526ca 106 emg_out0 = 0;
dolcaer 11:46a859e526ca 107 emg_out1 = 0;
dolcaer 11:46a859e526ca 108 emg_out2 = 0;
dolcaer 11:46a859e526ca 109 } else if (debug_tick < 65) {
dolcaer 11:46a859e526ca 110 emg_out0 = 0;
dolcaer 11:46a859e526ca 111 emg_out1 = 1;
dolcaer 11:46a859e526ca 112 emg_out2 = 0;
dolcaer 11:46a859e526ca 113 } else {
dolcaer 11:46a859e526ca 114 emg_out0 = 0;
dolcaer 11:46a859e526ca 115 emg_out1 = 0;
dolcaer 11:46a859e526ca 116 emg_out2 = 0;
dolcaer 11:46a859e526ca 117 }*/
AeroKev 9:70cab4bf38a5 118
AeroKev 9:70cab4bf38a5 119 // Calculate the new position using the EMG output and the current position
dolcaer 11:46a859e526ca 120 bool pushing;
dolcaer 11:46a859e526ca 121 newPos(emg_out0, emg_out1, emg_out2, a, b, x, y, pushing);
dolcaer 11:46a859e526ca 122 // pc2.printf("O0: %f | O1: %f | O2: %f\r\n",emg_out0, emg_out1, emg_out2);
AeroKev 10:3cee5adfbd72 123
AeroKev 10:3cee5adfbd72 124 Angles2Point(x,y,a,b);
AeroKev 10:3cee5adfbd72 125
dolcaer 11:46a859e526ca 126 if (pushing)
dolcaer 11:46a859e526ca 127 motorSpeed = 1;
dolcaer 11:46a859e526ca 128 else if(a < -20 || a > 20)
dolcaer 11:46a859e526ca 129 motorSpeed = 0.14;
AeroKev 10:3cee5adfbd72 130 else
dolcaer 11:46a859e526ca 131 motorSpeed = 0.25;
AeroKev 10:3cee5adfbd72 132
AeroKev 9:70cab4bf38a5 133 // Rotate the motors
dolcaer 11:46a859e526ca 134 if (pushing)
dolcaer 11:46a859e526ca 135 push(x, y);
dolcaer 11:46a859e526ca 136 else
dolcaer 11:46a859e526ca 137 rotate(x,y);
dolcaer 11:46a859e526ca 138
dolcaer 11:46a859e526ca 139 debug_tick++;
dolcaer 0:5f9d8f2142b6 140 }
dolcaer 0:5f9d8f2142b6 141
AeroKev 9:70cab4bf38a5 142 /* Functions which get called by tickers */
AeroKev 9:70cab4bf38a5 143 void goMotor() { go_motor = true; }
AeroKev 7:aad7d5cdecd3 144
AeroKev 9:70cab4bf38a5 145 void goTick() { go_tick = true; }
AeroKev 7:aad7d5cdecd3 146
AeroKev 9:70cab4bf38a5 147 void goSample() { go_sample = true; }
AeroKev 8:7814ba9f801e 148
AeroKev 9:70cab4bf38a5 149 /* Function called by pressing the SW3 Button */
AeroKev 7:aad7d5cdecd3 150 void startAll()
AeroKev 7:aad7d5cdecd3 151 {
AeroKev 5:5339a7be9fb7 152 started = !started;
AeroKev 7:aad7d5cdecd3 153 if(started)
AeroKev 7:aad7d5cdecd3 154 movement_timer.attach(&goMotor, 0.01f);
AeroKev 7:aad7d5cdecd3 155 else {
AeroKev 7:aad7d5cdecd3 156 go_motor = false;
AeroKev 7:aad7d5cdecd3 157 PID_stop();
AeroKev 7:aad7d5cdecd3 158 movement_timer.detach();
AeroKev 7:aad7d5cdecd3 159 }
AeroKev 5:5339a7be9fb7 160 }
AeroKev 5:5339a7be9fb7 161
AeroKev 9:70cab4bf38a5 162 /* Function called by pressing the SW2 Button */
AeroKev 7:aad7d5cdecd3 163 void calibrate()
AeroKev 7:aad7d5cdecd3 164 {
AeroKev 8:7814ba9f801e 165 sample_timer.attach(&goSample, 0.002);
AeroKev 10:3cee5adfbd72 166 calc_timer.attach(&goTick, 0.15);
AeroKev 5:5339a7be9fb7 167 }
AeroKev 5:5339a7be9fb7 168
AeroKev 9:70cab4bf38a5 169 /* Initialize the buttons */
AeroKev 7:aad7d5cdecd3 170 void init()
AeroKev 7:aad7d5cdecd3 171 {
AeroKev 5:5339a7be9fb7 172 // Set the shutup and start buttons
AeroKev 5:5339a7be9fb7 173 start.mode(PullUp);
AeroKev 5:5339a7be9fb7 174 start.attach_deasserted(&startAll);
AeroKev 5:5339a7be9fb7 175 start.setSampleFrequency();
AeroKev 7:aad7d5cdecd3 176
AeroKev 5:5339a7be9fb7 177 cali.mode(PullUp);
AeroKev 5:5339a7be9fb7 178 cali.attach_deasserted(&calibrate);
AeroKev 5:5339a7be9fb7 179 cali.setSampleFrequency();
AeroKev 7:aad7d5cdecd3 180
AeroKev 9:70cab4bf38a5 181 pc2.printf("Buttons done\r\n\r\n");
AeroKev 5:5339a7be9fb7 182 }
AeroKev 5:5339a7be9fb7 183
AeroKev 9:70cab4bf38a5 184 /* The main function
AeroKev 9:70cab4bf38a5 185 Initializes every library
AeroKev 9:70cab4bf38a5 186 Starts the timers
AeroKev 9:70cab4bf38a5 187 Has a while-loop which calls functions on a timer
AeroKev 9:70cab4bf38a5 188 */
dolcaer 0:5f9d8f2142b6 189 int main()
dolcaer 0:5f9d8f2142b6 190 {
AeroKev 6:4f8760baa448 191 init();
AeroKev 9:70cab4bf38a5 192
AeroKev 9:70cab4bf38a5 193 /* Initialize libraries */
AeroKev 6:4f8760baa448 194 double a,b;
AeroKev 6:4f8760baa448 195 IBC_init(a,b);
AeroKev 9:70cab4bf38a5 196 pc2.printf("(--- In Between Controller Initialized ---)\r\n");
AeroKev 9:70cab4bf38a5 197
AeroKev 9:70cab4bf38a5 198 PID_init();
AeroKev 9:70cab4bf38a5 199 pc2.printf("(--- PID Controller Initialized ---)\r\n");
AeroKev 7:aad7d5cdecd3 200
AeroKev 7:aad7d5cdecd3 201 movement_timer.attach(&goMotor, 0.01f);
AeroKev 7:aad7d5cdecd3 202
AeroKev 7:aad7d5cdecd3 203 rotate(a,b);
AeroKev 7:aad7d5cdecd3 204
AeroKev 9:70cab4bf38a5 205 /* The main while-loop */
AeroKev 7:aad7d5cdecd3 206 while(1) {
AeroKev 9:70cab4bf38a5 207 // The motor timer
AeroKev 7:aad7d5cdecd3 208 if(go_motor) {
AeroKev 7:aad7d5cdecd3 209 go_motor = false;
AeroKev 10:3cee5adfbd72 210 moveTick(motorSpeed);
AeroKev 7:aad7d5cdecd3 211 }
AeroKev 9:70cab4bf38a5 212 // The sample timer
AeroKev 8:7814ba9f801e 213 if(go_sample) {
AeroKev 8:7814ba9f801e 214 go_sample = false;
AeroKev 8:7814ba9f801e 215 sample(emg_out0, emg_out1, emg_out2);
AeroKev 8:7814ba9f801e 216 }
AeroKev 9:70cab4bf38a5 217 // The tick timer
AeroKev 7:aad7d5cdecd3 218 if(go_tick) {
AeroKev 7:aad7d5cdecd3 219 go_tick = false;
AeroKev 7:aad7d5cdecd3 220 tick();
AeroKev 7:aad7d5cdecd3 221 }
AeroKev 7:aad7d5cdecd3 222 }
dolcaer 0:5f9d8f2142b6 223 }