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@7:aad7d5cdecd3, 2015-10-21 (annotated)

Committer:
AeroKev
Date:
Wed Oct 21 13:46:28 2015 +0000
Revision:
7:aad7d5cdecd3
Parent:
6:4f8760baa448
Child:
8:7814ba9f801e
a

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 5:5339a7be9fb7 12 PinDetect cali(SW2);
AeroKev 5:5339a7be9fb7 13 PinDetect start(SW3);
AeroKev 5:5339a7be9fb7 14 int counter = 0;
AeroKev 7:aad7d5cdecd3 15 bool started = true;
AeroKev 5:5339a7be9fb7 16 bool calibrating = false;
AeroKev 7:aad7d5cdecd3 17 bool go_motor = true;
AeroKev 7:aad7d5cdecd3 18 bool go_tick = false;
dolcaer 0:5f9d8f2142b6 19
AeroKev 7:aad7d5cdecd3 20 double generateSample()
AeroKev 7:aad7d5cdecd3 21 {
AeroKev 5:5339a7be9fb7 22 counter++;
AeroKev 7:aad7d5cdecd3 23 if(counter < 10)
AeroKev 5:5339a7be9fb7 24 return 0.0;
AeroKev 7:aad7d5cdecd3 25 else if(counter < 50)
AeroKev 5:5339a7be9fb7 26 return 0.1;
AeroKev 7:aad7d5cdecd3 27 else if(counter < 100)
AeroKev 7:aad7d5cdecd3 28 return 0.2;
AeroKev 7:aad7d5cdecd3 29 else if(counter < 150)
AeroKev 7:aad7d5cdecd3 30 return 0.3;
AeroKev 7:aad7d5cdecd3 31 else if(counter < 200)
AeroKev 7:aad7d5cdecd3 32 return 0.4;
AeroKev 7:aad7d5cdecd3 33 else if(counter < 250)
AeroKev 7:aad7d5cdecd3 34 return 0.5;
AeroKev 5:5339a7be9fb7 35 return 0.0;
AeroKev 5:5339a7be9fb7 36 }
AeroKev 5:5339a7be9fb7 37
AeroKev 7:aad7d5cdecd3 38 void tick()
AeroKev 7:aad7d5cdecd3 39 {
AeroKev 5:5339a7be9fb7 40 if(!started) return;
AeroKev 3:0634f55c2021 41 // EMG Reader
AeroKev 5:5339a7be9fb7 42 double output0 = generateSample();
AeroKev 5:5339a7be9fb7 43 double output1 = 0.0;
AeroKev 5:5339a7be9fb7 44 double output2 = 0.0;
AeroKev 7:aad7d5cdecd3 45
AeroKev 3:0634f55c2021 46 // In Between Controller
AeroKev 4:e021dacffa24 47 double lastA, lastB, nextA, nextB;
AeroKev 4:e021dacffa24 48 getCurrent(lastA, lastB);
AeroKev 7:aad7d5cdecd3 49
AeroKev 5:5339a7be9fb7 50 newPos(output0, output1, output2, lastA, lastB, nextA, nextB);
AeroKev 7:aad7d5cdecd3 51
AeroKev 7:aad7d5cdecd3 52 pc2.printf("CURRENT: %d deg | %d deg\r\nNEW: %d deg | %d deg\r\n",rad2deg(lastA), rad2deg(lastB), rad2deg(nextA), rad2deg(nextB));
AeroKev 4:e021dacffa24 53 // Rotate the motors
AeroKev 6:4f8760baa448 54 rotate(nextA,nextB);
dolcaer 0:5f9d8f2142b6 55 }
dolcaer 0:5f9d8f2142b6 56
AeroKev 7:aad7d5cdecd3 57 void goMotor()
AeroKev 7:aad7d5cdecd3 58 {
AeroKev 7:aad7d5cdecd3 59 go_motor = true;
AeroKev 7:aad7d5cdecd3 60 }
AeroKev 7:aad7d5cdecd3 61
AeroKev 7:aad7d5cdecd3 62 void goTick()
AeroKev 7:aad7d5cdecd3 63 {
AeroKev 7:aad7d5cdecd3 64 go_tick = true;
AeroKev 7:aad7d5cdecd3 65 }
AeroKev 7:aad7d5cdecd3 66
AeroKev 7:aad7d5cdecd3 67 void startAll()
AeroKev 7:aad7d5cdecd3 68 {
AeroKev 5:5339a7be9fb7 69 started = !started;
AeroKev 7:aad7d5cdecd3 70 if(started)
AeroKev 7:aad7d5cdecd3 71 movement_timer.attach(&goMotor, 0.01f);
AeroKev 7:aad7d5cdecd3 72 else {
AeroKev 7:aad7d5cdecd3 73 go_motor = false;
AeroKev 7:aad7d5cdecd3 74 PID_stop();
AeroKev 7:aad7d5cdecd3 75 movement_timer.detach();
AeroKev 7:aad7d5cdecd3 76 }
AeroKev 5:5339a7be9fb7 77 }
AeroKev 5:5339a7be9fb7 78
AeroKev 7:aad7d5cdecd3 79 int ding = 0;
AeroKev 7:aad7d5cdecd3 80 void calibrate()
AeroKev 7:aad7d5cdecd3 81 {
AeroKev 7:aad7d5cdecd3 82 ding++;
AeroKev 7:aad7d5cdecd3 83 double a,b;
AeroKev 7:aad7d5cdecd3 84 if(ding == 1) {
AeroKev 7:aad7d5cdecd3 85 //sample_timer.attach(&goTick, 0.002);
AeroKev 7:aad7d5cdecd3 86 Point2Angles(0,20,a,b);
AeroKev 7:aad7d5cdecd3 87 double radA, radB;
AeroKev 7:aad7d5cdecd3 88 float pX, pY;
AeroKev 7:aad7d5cdecd3 89 //pc2.printf("Moving to (0,20) A: %d | B: %d\r\n",rad2deg(a),rad2deg(b));
AeroKev 7:aad7d5cdecd3 90 rotate(a,b);
AeroKev 7:aad7d5cdecd3 91 getCurrent(radA, radB);
AeroKev 7:aad7d5cdecd3 92 Angles2Point(-radA + getOffset(1),-radB + getOffset(2),pX,pY);
AeroKev 7:aad7d5cdecd3 93 pc2.printf("Px1: %f | Py1: %f\r\n",pX,pY);
AeroKev 7:aad7d5cdecd3 94 }
AeroKev 7:aad7d5cdecd3 95
AeroKev 7:aad7d5cdecd3 96 if(ding == 2) {
AeroKev 7:aad7d5cdecd3 97 Point2Angles(-20,20,a,b);
AeroKev 7:aad7d5cdecd3 98 double radA, radB;
AeroKev 7:aad7d5cdecd3 99 float pX, pY;
AeroKev 7:aad7d5cdecd3 100 //pc2.printf("Moving to (-20,20) A: %d | B: %d\r\n",rad2deg(a),rad2deg(b));
AeroKev 7:aad7d5cdecd3 101 rotate(a,b);
AeroKev 7:aad7d5cdecd3 102 getCurrent(radA, radB);
AeroKev 7:aad7d5cdecd3 103 Angles2Point(-radA + getOffset(1),-radB + getOffset(2),pX,pY);
AeroKev 7:aad7d5cdecd3 104 pc2.printf("Px2: %f | Py2: %f\r\n",pX,pY);
AeroKev 7:aad7d5cdecd3 105 ding = 0;
AeroKev 7:aad7d5cdecd3 106 }
AeroKev 5:5339a7be9fb7 107 }
AeroKev 5:5339a7be9fb7 108
AeroKev 7:aad7d5cdecd3 109 void init()
AeroKev 7:aad7d5cdecd3 110 {
AeroKev 5:5339a7be9fb7 111 // Set the shutup and start buttons
AeroKev 5:5339a7be9fb7 112 start.mode(PullUp);
AeroKev 5:5339a7be9fb7 113 start.attach_deasserted(&startAll);
AeroKev 5:5339a7be9fb7 114 start.setSampleFrequency();
AeroKev 7:aad7d5cdecd3 115
AeroKev 5:5339a7be9fb7 116 cali.mode(PullUp);
AeroKev 5:5339a7be9fb7 117 cali.attach_deasserted(&calibrate);
AeroKev 5:5339a7be9fb7 118 cali.setSampleFrequency();
AeroKev 7:aad7d5cdecd3 119
AeroKev 6:4f8760baa448 120 pc2.printf("Buttons done\r\nStarting Initialization...\r\n\r\n");
AeroKev 5:5339a7be9fb7 121 }
AeroKev 5:5339a7be9fb7 122
dolcaer 0:5f9d8f2142b6 123 int main()
dolcaer 0:5f9d8f2142b6 124 {
AeroKev 6:4f8760baa448 125 init();
AeroKev 6:4f8760baa448 126 pc2.printf("STARTED!");
AeroKev 5:5339a7be9fb7 127 // Initialize libraries
AeroKev 6:4f8760baa448 128 double a,b;
AeroKev 6:4f8760baa448 129 IBC_init(a,b);
AeroKev 6:4f8760baa448 130 PID_init(a,b);
AeroKev 7:aad7d5cdecd3 131
AeroKev 7:aad7d5cdecd3 132 //movement_timer.attach(&moveTick, 0.01f);
AeroKev 7:aad7d5cdecd3 133
AeroKev 7:aad7d5cdecd3 134 pc2.printf("A: %f | B: %f\r\n",a,b);
AeroKev 7:aad7d5cdecd3 135
AeroKev 7:aad7d5cdecd3 136 movement_timer.attach(&goMotor, 0.01f);
AeroKev 7:aad7d5cdecd3 137
AeroKev 7:aad7d5cdecd3 138 rotate(a,b);
AeroKev 7:aad7d5cdecd3 139
AeroKev 7:aad7d5cdecd3 140 while(1) {
AeroKev 7:aad7d5cdecd3 141 if(go_motor) {
AeroKev 7:aad7d5cdecd3 142 go_motor = false;
AeroKev 7:aad7d5cdecd3 143 moveTick();
AeroKev 7:aad7d5cdecd3 144 }
AeroKev 7:aad7d5cdecd3 145 if(go_tick) {
AeroKev 7:aad7d5cdecd3 146 go_tick = false;
AeroKev 7:aad7d5cdecd3 147 tick();
AeroKev 7:aad7d5cdecd3 148 }
AeroKev 7:aad7d5cdecd3 149 }
dolcaer 0:5f9d8f2142b6 150 }