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@20:ebeb18494f3e, 2015-11-03 (annotated)

Committer:
AeroKev
Date:
Tue Nov 03 10:16:46 2015 +0000
Revision:
20:ebeb18494f3e
Parent:
19:7c356ec0fae0
Child:
21:9de9e0a73e57
Last Commit

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