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@4:680f775a3703, 2015-10-18 (annotated)

Committer:
mganseij
Date:
Sun Oct 18 14:13:51 2015 +0000
Revision:
4:680f775a3703
Parent:
3:70f78cfc0f25
Child:
5:e52055ff2bfe
fixed a bug with stepping of the horizontal and vertical request. Prior to this commit they would never reach the value of 0.5 as the wrong variables were changed.

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 1:dafb63606b66 26
ewoud 0:ca94aa9bf823 27 int main() {
ewoud 0:ca94aa9bf823 28
ewoud 0:ca94aa9bf823 29 // initialize (defined in inits.h)
ewoud 0:ca94aa9bf823 30 initMotors();
ewoud 0:ca94aa9bf823 31 initPIDs();
ewoud 0:ca94aa9bf823 32
ewoud 0:ca94aa9bf823 33 outofboundsled=1;
ewoud 0:ca94aa9bf823 34 edgeleft.mode(PullUp);
ewoud 0:ca94aa9bf823 35 edgeright.mode(PullUp);
ewoud 1:dafb63606b66 36
ewoud 1:dafb63606b66 37 // interrupts
ewoud 0:ca94aa9bf823 38 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 3:70f78cfc0f25 39 cali_button.rise(&calibratego);
ewoud 0:ca94aa9bf823 40 startButton.rise(&systemStart);
ewoud 0:ca94aa9bf823 41 stopButton.rise(&systemStop);
ewoud 0:ca94aa9bf823 42
ewoud 0:ca94aa9bf823 43 while (true){
ewoud 3:70f78cfc0f25 44 if (calibrate_go==true){
ewoud 3:70f78cfc0f25 45 calibrate();
ewoud 3:70f78cfc0f25 46
ewoud 3:70f78cfc0f25 47
ewoud 3:70f78cfc0f25 48 }
ewoud 0:ca94aa9bf823 49 if (goFlag==true && systemOn==true){
ewoud 0:ca94aa9bf823 50 /*********** Contents *************/
ewoud 0:ca94aa9bf823 51 // 1) get emg signal
ewoud 0:ca94aa9bf823 52 // 2) calculate desired angle velocities
ewoud 0:ca94aa9bf823 53 // 3) calculate current angle velocities
ewoud 1:dafb63606b66 54 // 4) pid controller output
ewoud 0:ca94aa9bf823 55 // 5) user output
ewoud 0:ca94aa9bf823 56
ewoud 1:dafb63606b66 57 // **************
ewoud 0:ca94aa9bf823 58 // get emg signal
ewoud 0:ca94aa9bf823 59 sample_filter();
ewoud 0:ca94aa9bf823 60
ewoud 1:dafb63606b66 61 // determine if forward or backward signal is stronger
ewoud 3:70f78cfc0f25 62 if (y1 > y2){
ewoud 3:70f78cfc0f25 63 horrequest = y1;
ewoud 3:70f78cfc0f25 64 }
ewoud 3:70f78cfc0f25 65 else {
ewoud 3:70f78cfc0f25 66 horrequest = -y2;
ewoud 3:70f78cfc0f25 67 }
ewoud 3:70f78cfc0f25 68 if (y3 > y4){
ewoud 3:70f78cfc0f25 69 verrequest = y3;
ewoud 0:ca94aa9bf823 70 }
ewoud 0:ca94aa9bf823 71 else {
ewoud 3:70f78cfc0f25 72 verrequest = -y4;
ewoud 0:ca94aa9bf823 73 }
ewoud 3:70f78cfc0f25 74
ewoud 3:70f78cfc0f25 75 // perform stepping between boundries
mganseij 4:680f775a3703 76 if(horrequest < -grenshoog){horrequest=-1;} else if(horrequest>=-grenshoog and horrequest<-grenslaag){horrequest=-0.5;}
mganseij 4:680f775a3703 77 if(horrequest > grenshoog){horrequest=1; } else if(horrequest>= grenslaag and horrequest<grenshoog){horrequest=0.5;}
ewoud 3:70f78cfc0f25 78 else {horrequest=0;}
ewoud 3:70f78cfc0f25 79
mganseij 4:680f775a3703 80 if(verrequest < -grenshoog){verrequest=-1;} else if(verrequest>=-grenshoog and verrequest<-grenslaag){verrequest=-0.5;}
mganseij 4:680f775a3703 81 if(verrequest > grenshoog){verrequest=1; } else if(verrequest>= grenslaag and verrequest<grenshoog){verrequest=0.5;}
ewoud 3:70f78cfc0f25 82 else {verrequest=0;}
ewoud 3:70f78cfc0f25 83
ewoud 3:70f78cfc0f25 84 horrequest=horrequest*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
ewoud 3:70f78cfc0f25 85 verrequest=verrequest*maxspeed;
ewoud 3:70f78cfc0f25 86
ewoud 1:dafb63606b66 87
ewoud 1:dafb63606b66 88 // ***************
ewoud 0:ca94aa9bf823 89 // calculate required rotational velocity from the requested horizontal velocity
ewoud 1:dafb63606b66 90 // first get the current position from the motor encoders and make them start at 45 degree.
ewoud 1:dafb63606b66 91 leftAngle=(leftQei.getPulses()/round)*360+45;
ewoud 0:ca94aa9bf823 92 rightAngle=(rightQei.getPulses()/round)*360+45;
ewoud 0:ca94aa9bf823 93
ewoud 1:dafb63606b66 94 // trigonometry to get xy position from angles (cm)
ewoud 0:ca94aa9bf823 95 currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
ewoud 0:ca94aa9bf823 96 currentY = tan(leftAngle*M_PI/180)*currentX;
ewoud 0:ca94aa9bf823 97
ewoud 0:ca94aa9bf823 98 // restrict motion if edges are touched
ewoud 0:ca94aa9bf823 99 if (edgeleft==0){
ewoud 3:70f78cfc0f25 100 if (horrequest < 0){horrequest=0; pc.printf("hit left edge \n\r");}
ewoud 0:ca94aa9bf823 101 }
ewoud 0:ca94aa9bf823 102 if (edgeright==0){
ewoud 3:70f78cfc0f25 103 if (horrequest > 0){horrequest=0; pc.printf("hit right edge \n\r");}
ewoud 3:70f78cfc0f25 104 }
ewoud 3:70f78cfc0f25 105 if (edgetop==0){
ewoud 3:70f78cfc0f25 106 if (verrequest > 0){verrequest=0; pc.printf("hit top edge \n\r");}
ewoud 3:70f78cfc0f25 107 }
ewoud 3:70f78cfc0f25 108 if (edgebottom==0){
ewoud 3:70f78cfc0f25 109 if (verrequest > 0){verrequest=0; pc.printf("hit bottom edge \n\r");}
ewoud 0:ca94aa9bf823 110 }
ewoud 0:ca94aa9bf823 111
ewoud 1:dafb63606b66 112 // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
ewoud 3:70f78cfc0f25 113 toX=currentX+horrequest*RATE;
ewoud 3:70f78cfc0f25 114 toY=currentY+verrequest*RATE; // should be vertical request*RATE
ewoud 0:ca94aa9bf823 115
ewoud 1:dafb63606b66 116 // trigonometry to get angles from xy new position (degree)
ewoud 0:ca94aa9bf823 117 toLeftAngle = atan(toY/toX)*180/M_PI;
ewoud 0:ca94aa9bf823 118 toRightAngle = atan(toY/(l-toX))*180/M_PI;
ewoud 0:ca94aa9bf823 119
ewoud 0:ca94aa9bf823 120 // restrict motion if angles out-of-bound
ewoud 0:ca94aa9bf823 121 if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 122 if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 123 if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 124 if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
ewoud 0:ca94aa9bf823 125
ewoud 1:dafb63606b66 126 // restrict motion if position is out of reach of the arms
ewoud 0:ca94aa9bf823 127 //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
ewoud 0:ca94aa9bf823 128 //return 0;
ewoud 0:ca94aa9bf823 129 //}
ewoud 0:ca94aa9bf823 130 //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
ewoud 0:ca94aa9bf823 131 //return 0;
ewoud 0:ca94aa9bf823 132 //}
ewoud 0:ca94aa9bf823 133
ewoud 1:dafb63606b66 134 // calculate the neccesairy velocities to make these angles happen (degree/sec)
ewoud 1:dafb63606b66 135 leftRequest=(toLeftAngle-leftAngle)/RATE;
ewoud 1:dafb63606b66 136 rightRequest=(toRightAngle-rightAngle)/RATE;
ewoud 0:ca94aa9bf823 137
ewoud 0:ca94aa9bf823 138 // set the setpoint to the pid controller
ewoud 0:ca94aa9bf823 139 leftController.setSetPoint(leftRequest);
ewoud 0:ca94aa9bf823 140 rightController.setSetPoint(rightRequest);
ewoud 0:ca94aa9bf823 141
ewoud 1:dafb63606b66 142 // **************
ewoud 0:ca94aa9bf823 143 // Velocity calculation
ewoud 1:dafb63606b66 144 // left, differentiate from encoders
ewoud 1:dafb63606b66 145 leftPulses = leftQei.getPulses()/round*360; // (degree)
ewoud 1:dafb63606b66 146 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
ewoud 0:ca94aa9bf823 147 leftPrevPulses = leftPulses;
ewoud 1:dafb63606b66 148 leftController.setProcessValue(leftVelocity); // set PID process value
ewoud 0:ca94aa9bf823 149
ewoud 0:ca94aa9bf823 150 // right
ewoud 1:dafb63606b66 151 rightPulses = rightQei.getPulses()/round*360; // (degree)
ewoud 1:dafb63606b66 152 rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
ewoud 0:ca94aa9bf823 153 rightPrevPulses = rightPulses;
ewoud 1:dafb63606b66 154 rightController.setProcessValue(rightVelocity); // set PID process value
ewoud 0:ca94aa9bf823 155
ewoud 0:ca94aa9bf823 156
ewoud 1:dafb63606b66 157 // **************
ewoud 0:ca94aa9bf823 158 // PID control output
ewoud 0:ca94aa9bf823 159 // left
ewoud 0:ca94aa9bf823 160
ewoud 0:ca94aa9bf823 161 leftPwmDuty = leftController.compute();
ewoud 1:dafb63606b66 162 if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
ewoud 0:ca94aa9bf823 163 leftDirection = 0;
ewoud 0:ca94aa9bf823 164 leftMotor = -leftPwmDuty;
ewoud 0:ca94aa9bf823 165 }
ewoud 0:ca94aa9bf823 166 else {
ewoud 0:ca94aa9bf823 167 leftDirection = 1;
ewoud 0:ca94aa9bf823 168 leftMotor = leftPwmDuty;
ewoud 0:ca94aa9bf823 169 }
ewoud 0:ca94aa9bf823 170
ewoud 0:ca94aa9bf823 171 // right
ewoud 0:ca94aa9bf823 172 rightPwmDuty = rightController.compute();
ewoud 1:dafb63606b66 173 if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
ewoud 0:ca94aa9bf823 174 rightDirection = 1;
ewoud 0:ca94aa9bf823 175 rightMotor = -rightPwmDuty;
ewoud 0:ca94aa9bf823 176 }
ewoud 0:ca94aa9bf823 177 else {
ewoud 0:ca94aa9bf823 178 rightDirection = 0;
ewoud 0:ca94aa9bf823 179 rightMotor = rightPwmDuty;
ewoud 0:ca94aa9bf823 180 }
ewoud 0:ca94aa9bf823 181
ewoud 1:dafb63606b66 182 // ***************
ewoud 0:ca94aa9bf823 183 // User feedback
ewoud 3:70f78cfc0f25 184 scope.set(0, horrequest);
ewoud 0:ca94aa9bf823 185 scope.set(1, leftPwmDuty);
ewoud 0:ca94aa9bf823 186 scope.set(2, leftVelocity);
ewoud 0:ca94aa9bf823 187 scope.set(3, currentX);
ewoud 0:ca94aa9bf823 188 scope.set(4, currentY);
ewoud 0:ca94aa9bf823 189 scope.send();
ewoud 1:dafb63606b66 190
ewoud 0:ca94aa9bf823 191 goFlag=false;
ewoud 0:ca94aa9bf823 192 }
ewoud 0:ca94aa9bf823 193 if(systemOn==false)
ewoud 0:ca94aa9bf823 194 {
ewoud 0:ca94aa9bf823 195 leftMotor=0;
ewoud 0:ca94aa9bf823 196 rightMotor=0;
ewoud 0:ca94aa9bf823 197 }
ewoud 0:ca94aa9bf823 198 }
ewoud 0:ca94aa9bf823 199
ewoud 0:ca94aa9bf823 200 }