Numero Uno / Mbed 2 deprecated TheProgram

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:   EMG HIDScope PID QEI mbed TextLCD

main.cpp@0:ca94aa9bf823, 2015-10-13 (annotated)

Committer:
ewoud
Date:
Tue Oct 13 17:46:45 2015 +0000
Revision:
0:ca94aa9bf823
Child:
1:dafb63606b66
working emg + xy to rotation + p controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 0:ca94aa9bf823 1 //****************************************************************************/
ewoud 0:ca94aa9bf823 2 // Includes
ewoud 0:ca94aa9bf823 3 #include "mbed.h"
ewoud 0:ca94aa9bf823 4 #include "PID.h"
ewoud 0:ca94aa9bf823 5 #include "QEI.h"
ewoud 0:ca94aa9bf823 6 #include "HIDScope.h"
ewoud 0:ca94aa9bf823 7 #include "biquadFilter.h"
ewoud 0:ca94aa9bf823 8 #include "inits.h" // all globals, pin and variable initialization
ewoud 0:ca94aa9bf823 9 #include "EMG.h"
ewoud 0:ca94aa9bf823 10
ewoud 0:ca94aa9bf823 11 void setGoFlag(){
ewoud 0:ca94aa9bf823 12 if (goFlag==true){
ewoud 0:ca94aa9bf823 13 //pc.printf("rate too high, error in setGoFlag \n\r");
ewoud 0:ca94aa9bf823 14 // flag is already set true: code too slow or frequency too high
ewoud 0:ca94aa9bf823 15 }
ewoud 0:ca94aa9bf823 16 goFlag=true;
ewoud 0:ca94aa9bf823 17 }
ewoud 0:ca94aa9bf823 18
ewoud 0:ca94aa9bf823 19 void systemStart(){
ewoud 0:ca94aa9bf823 20 systemOn=true;
ewoud 0:ca94aa9bf823 21 }
ewoud 0:ca94aa9bf823 22 void systemStop(){
ewoud 0:ca94aa9bf823 23 systemOn=false;
ewoud 0:ca94aa9bf823 24 pc.printf("system stopped\n\r");
ewoud 0:ca94aa9bf823 25 }
ewoud 0:ca94aa9bf823 26 void stoptop(){
ewoud 0:ca94aa9bf823 27 pc.printf("stop top\n\r");
ewoud 0:ca94aa9bf823 28 }
ewoud 0:ca94aa9bf823 29 void gotop(){
ewoud 0:ca94aa9bf823 30 pc.printf("go top\n\r");
ewoud 0:ca94aa9bf823 31
ewoud 0:ca94aa9bf823 32 }
ewoud 0:ca94aa9bf823 33 bool skipvelocity=false;
ewoud 0:ca94aa9bf823 34 int main() {
ewoud 0:ca94aa9bf823 35
ewoud 0:ca94aa9bf823 36 // initialize (defined in inits.h)
ewoud 0:ca94aa9bf823 37 initMotors();
ewoud 0:ca94aa9bf823 38 initPIDs();
ewoud 0:ca94aa9bf823 39
ewoud 0:ca94aa9bf823 40 outofboundsled=1;
ewoud 0:ca94aa9bf823 41 edgeleft.mode(PullUp);
ewoud 0:ca94aa9bf823 42 edgeright.mode(PullUp);
ewoud 0:ca94aa9bf823 43 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 0:ca94aa9bf823 44 //T1.attach(&samplego, (float)1/Fs);
ewoud 0:ca94aa9bf823 45
ewoud 0:ca94aa9bf823 46 cali_button.rise(&calibrate);
ewoud 0:ca94aa9bf823 47 startButton.rise(&systemStart);
ewoud 0:ca94aa9bf823 48 stopButton.rise(&systemStop);
ewoud 0:ca94aa9bf823 49 //edgetop.rise(&stoptop);
ewoud 0:ca94aa9bf823 50 //edgetop.fall(&gotop);
ewoud 0:ca94aa9bf823 51
ewoud 0:ca94aa9bf823 52
ewoud 0:ca94aa9bf823 53 endTimer.start(); //Run for 100 seconds.
ewoud 0:ca94aa9bf823 54 while (true){
ewoud 0:ca94aa9bf823 55
ewoud 0:ca94aa9bf823 56 if (goFlag==true && systemOn==true){
ewoud 0:ca94aa9bf823 57 /*********** Contents *************/
ewoud 0:ca94aa9bf823 58 // 1) get emg signal
ewoud 0:ca94aa9bf823 59 // 2) calculate desired angle velocities
ewoud 0:ca94aa9bf823 60 // 3) calculate current angle velocities
ewoud 0:ca94aa9bf823 61 // 4) pid controller
ewoud 0:ca94aa9bf823 62 // 5) user output
ewoud 0:ca94aa9bf823 63
ewoud 0:ca94aa9bf823 64
ewoud 0:ca94aa9bf823 65 // get emg signal
ewoud 0:ca94aa9bf823 66 sample_filter();
ewoud 0:ca94aa9bf823 67
ewoud 0:ca94aa9bf823 68 request_pos=y1;
ewoud 0:ca94aa9bf823 69 request_neg=y2;
ewoud 0:ca94aa9bf823 70
ewoud 0:ca94aa9bf823 71 // determine if forward or backward signal is stronger and set reference
ewoud 0:ca94aa9bf823 72 if (request_pos > request_neg){
ewoud 0:ca94aa9bf823 73 request = request_pos;
ewoud 0:ca94aa9bf823 74 }
ewoud 0:ca94aa9bf823 75 else {
ewoud 0:ca94aa9bf823 76 request = -request_neg;
ewoud 0:ca94aa9bf823 77 }
ewoud 0:ca94aa9bf823 78 request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
ewoud 0:ca94aa9bf823 79
ewoud 0:ca94aa9bf823 80 // calculate required rotational velocity from the requested horizontal velocity
ewoud 0:ca94aa9bf823 81 // first get the current position from the motor encoders
ewoud 0:ca94aa9bf823 82 // make them start at 45 degree.
ewoud 0:ca94aa9bf823 83 leftAngle=(leftQei.getPulses()/round)*360+45;
ewoud 0:ca94aa9bf823 84 rightAngle=(rightQei.getPulses()/round)*360+45;
ewoud 0:ca94aa9bf823 85
ewoud 0:ca94aa9bf823 86 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
ewoud 0:ca94aa9bf823 87 currentY = tan(leftAngle*M_PI/180)*currentX;
ewoud 0:ca94aa9bf823 88
ewoud 0:ca94aa9bf823 89 // restrict motion if edges are touched
ewoud 0:ca94aa9bf823 90 if (edgeleft==0){
ewoud 0:ca94aa9bf823 91 if (request < 0){request=0; pc.printf("hit left edge \n\r");}
ewoud 0:ca94aa9bf823 92 }
ewoud 0:ca94aa9bf823 93 if (edgeright==0){
ewoud 0:ca94aa9bf823 94 if (request > 0){request=0; pc.printf("hit right edge \n\r");}
ewoud 0:ca94aa9bf823 95 }
ewoud 0:ca94aa9bf823 96
ewoud 0:ca94aa9bf823 97 // calculate the position to go to according the the current position + the distance that should be covered in this timestep
ewoud 0:ca94aa9bf823 98 toX=currentX+request*RATE;
ewoud 0:ca94aa9bf823 99 toY=currentY+0*RATE; // should be vertical request*RATE
ewoud 0:ca94aa9bf823 100
ewoud 0:ca94aa9bf823 101 toLeftAngle = atan(toY/toX)*180/M_PI;
ewoud 0:ca94aa9bf823 102 toRightAngle = atan(toY/(l-toX))*180/M_PI;
ewoud 0:ca94aa9bf823 103
ewoud 0:ca94aa9bf823 104 // restrict motion if angles out-of-bound
ewoud 0:ca94aa9bf823 105 if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 106 if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 107 if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 108 if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 109
ewoud 0:ca94aa9bf823 110 //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
ewoud 0:ca94aa9bf823 111 //return 0;
ewoud 0:ca94aa9bf823 112 //}
ewoud 0:ca94aa9bf823 113 //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
ewoud 0:ca94aa9bf823 114 //return 0;
ewoud 0:ca94aa9bf823 115 //}
ewoud 0:ca94aa9bf823 116
ewoud 0:ca94aa9bf823 117 // calculate how much the angles should change in this timestep
ewoud 0:ca94aa9bf823 118 leftDeltaAngle=(toLeftAngle-leftAngle);
ewoud 0:ca94aa9bf823 119 rightDeltaAngle=(toRightAngle-rightAngle);
ewoud 0:ca94aa9bf823 120
ewoud 0:ca94aa9bf823 121 // calculate the neccesairy velocities to make these angles happen.
ewoud 0:ca94aa9bf823 122 leftRequest=(leftDeltaAngle/RATE); // degrees/sec
ewoud 0:ca94aa9bf823 123 rightRequest=(rightDeltaAngle/RATE); // degrees/sec
ewoud 0:ca94aa9bf823 124
ewoud 0:ca94aa9bf823 125 // set the setpoint to the pid controller
ewoud 0:ca94aa9bf823 126 leftController.setSetPoint(leftRequest);
ewoud 0:ca94aa9bf823 127 rightController.setSetPoint(rightRequest);
ewoud 0:ca94aa9bf823 128
ewoud 0:ca94aa9bf823 129 // *******************
ewoud 0:ca94aa9bf823 130 // Velocity calculation
ewoud 0:ca94aa9bf823 131 // left
ewoud 0:ca94aa9bf823 132 leftPulses = leftQei.getPulses()/round*360; // in degree
ewoud 0:ca94aa9bf823 133 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; //degree /sec
ewoud 0:ca94aa9bf823 134 leftVelocity = leftVelocity; // scale to 0 - 1, max velocity = 20 degrees /sec
ewoud 0:ca94aa9bf823 135 //leftVelocity = leftVelocityfilter.step(leftVelocity);
ewoud 0:ca94aa9bf823 136 leftPrevPulses = leftPulses;
ewoud 0:ca94aa9bf823 137 leftController.setProcessValue(leftVelocity);
ewoud 0:ca94aa9bf823 138
ewoud 0:ca94aa9bf823 139 // right
ewoud 0:ca94aa9bf823 140 rightPulses = rightQei.getPulses()/round*360;
ewoud 0:ca94aa9bf823 141 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
ewoud 0:ca94aa9bf823 142 rightVelocity = rightVelocity; // scale to 0 - 1, max velocity = 4200
ewoud 0:ca94aa9bf823 143 //rightVelocity = rightVelocityfilter.step(rightVelocity);
ewoud 0:ca94aa9bf823 144 rightPrevPulses = rightPulses;
ewoud 0:ca94aa9bf823 145 rightController.setProcessValue(rightVelocity);
ewoud 0:ca94aa9bf823 146
ewoud 0:ca94aa9bf823 147
ewoud 0:ca94aa9bf823 148 // ***********
ewoud 0:ca94aa9bf823 149 // PID control output
ewoud 0:ca94aa9bf823 150 // left
ewoud 0:ca94aa9bf823 151
ewoud 0:ca94aa9bf823 152 leftPwmDuty = leftController.compute();
ewoud 0:ca94aa9bf823 153 if (leftPwmDuty < 0){
ewoud 0:ca94aa9bf823 154 leftDirection = 0;
ewoud 0:ca94aa9bf823 155 leftMotor = -leftPwmDuty;
ewoud 0:ca94aa9bf823 156 }
ewoud 0:ca94aa9bf823 157 else {
ewoud 0:ca94aa9bf823 158 leftDirection = 1;
ewoud 0:ca94aa9bf823 159 leftMotor = leftPwmDuty;
ewoud 0:ca94aa9bf823 160 }
ewoud 0:ca94aa9bf823 161
ewoud 0:ca94aa9bf823 162 // right
ewoud 0:ca94aa9bf823 163 rightPwmDuty = rightController.compute();
ewoud 0:ca94aa9bf823 164 if (rightPwmDuty < 0){
ewoud 0:ca94aa9bf823 165 rightDirection = 1;
ewoud 0:ca94aa9bf823 166 rightMotor = -rightPwmDuty;
ewoud 0:ca94aa9bf823 167 }
ewoud 0:ca94aa9bf823 168 else {
ewoud 0:ca94aa9bf823 169 rightDirection = 0;
ewoud 0:ca94aa9bf823 170 rightMotor = rightPwmDuty;
ewoud 0:ca94aa9bf823 171 }
ewoud 0:ca94aa9bf823 172
ewoud 0:ca94aa9bf823 173 // User feedback
ewoud 0:ca94aa9bf823 174 scope.set(0, leftRequest);
ewoud 0:ca94aa9bf823 175 scope.set(1, leftPwmDuty);
ewoud 0:ca94aa9bf823 176 scope.set(2, leftVelocity);
ewoud 0:ca94aa9bf823 177 scope.set(3, currentX);
ewoud 0:ca94aa9bf823 178 scope.set(4, currentY);
ewoud 0:ca94aa9bf823 179 scope.send();
ewoud 0:ca94aa9bf823 180 //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 0:ca94aa9bf823 181
ewoud 0:ca94aa9bf823 182 goFlag=false;
ewoud 0:ca94aa9bf823 183 }
ewoud 0:ca94aa9bf823 184 if(systemOn==false)
ewoud 0:ca94aa9bf823 185 {
ewoud 0:ca94aa9bf823 186 leftMotor=0;
ewoud 0:ca94aa9bf823 187 rightMotor=0;
ewoud 0:ca94aa9bf823 188 }
ewoud 0:ca94aa9bf823 189 }
ewoud 0:ca94aa9bf823 190
ewoud 0:ca94aa9bf823 191 }