Irnti Nousi / Mbed 2 deprecated Robot_control_v4

Group19-Biorobotics

Dependencies:   HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed

Fork of Robot_control_v3 by Fabian van Hummel

Controller.cpp@2:6b913f93bc0b, 2016-10-26 (annotated)

Committer:
fabian101
Date:
Wed Oct 26 12:37:37 2016 +0000
Revision:
2:6b913f93bc0b
Parent:
1:b9005f2aabf5
Child:
4:bfdcf3da7cff
26-10 group 19

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabian101 0:d935ea6f5c25 1 #include "initialize.h"
fabian101 0:d935ea6f5c25 2 #include "Controller.h"
fabian101 1:b9005f2aabf5 3
fabian101 0:d935ea6f5c25 4 int main() {
fabian101 0:d935ea6f5c25 5 calibrateButton.rise(&calibrate);
fabian101 2:6b913f93bc0b 6 motorControl.attach(&setFlag, RATE); // rate =0.001
fabian101 1:b9005f2aabf5 7 initMotors();
fabian101 1:b9005f2aabf5 8 initControllers();
fabian101 2:6b913f93bc0b 9 calibrate(); // calibrate position
fabian101 1:b9005f2aabf5 10 calibratePower(); // store max amplitude each emg signal
fabian101 0:d935ea6f5c25 11 run();
fabian101 0:d935ea6f5c25 12 }
fabian101 0:d935ea6f5c25 13
fabian101 0:d935ea6f5c25 14 void run() {
fabian101 0:d935ea6f5c25 15 while(true){
fabian101 2:6b913f93bc0b 16 if(flag){
fabian101 2:6b913f93bc0b 17 // returns filtered value
fabian101 2:6b913f93bc0b 18 r += signal.readValue(1)/SAMPLES_PER_AVERAGE;
fabian101 2:6b913f93bc0b 19 l += signal.readValue(2)/SAMPLES_PER_AVERAGE;
fabian101 2:6b913f93bc0b 20 u += signal.readValue(3)/SAMPLES_PER_AVERAGE;
fabian101 2:6b913f93bc0b 21 d += signal.readValue(4)/SAMPLES_PER_AVERAGE;
fabian101 2:6b913f93bc0b 22 }
fabian101 2:6b913f93bc0b 23 if(i=10){ // the filtering works on a frequency which is 10 times higher
fabian101 2:6b913f93bc0b 24
fabian101 2:6b913f93bc0b 25 // calculate r,l,u,d in a % of max
fabian101 2:6b913f93bc0b 26 double percentageR = r/rMax;
fabian101 2:6b913f93bc0b 27 double percentageL = l/lMax;
fabian101 2:6b913f93bc0b 28 double percentageU = u/uMax;
fabian101 2:6b913f93bc0b 29 double percentageD = d/dMax;
fabian101 2:6b913f93bc0b 30
fabian101 2:6b913f93bc0b 31 double minXThreshold;
fabian101 2:6b913f93bc0b 32 double minYThreshold;
fabian101 2:6b913f93bc0b 33
fabian101 2:6b913f93bc0b 34 r=0; l=0; u=0; d=0;
fabian101 2:6b913f93bc0b 35 // setting the flag here will cause the edge detection to be ignored if the flag is false for some reason
fabian101 2:6b913f93bc0b 36 // ignore the weaker signal,motorValue calculation, threshold set
fabian101 2:6b913f93bc0b 37 }
fabian101 0:d935ea6f5c25 38
fabian101 2:6b913f93bc0b 39 if ( percentageR > percentageL){ // horizontal speed request
fabian101 2:6b913f93bc0b 40 motorValueX = percentageR;
fabian101 2:6b913f93bc0b 41 minXThreshold = 0.2f*rMax; // set the threshold
fabian101 2:6b913f93bc0b 42 } else {motorValueX = -percentageL;
fabian101 2:6b913f93bc0b 43 minXThreshold = 0.2f*lMax;
fabian101 2:6b913f93bc0b 44 }
fabian101 1:b9005f2aabf5 45 if (percentageU > percentageD) { // vertical speed request
fabian101 2:6b913f93bc0b 46 motorValueY = percentageU;
fabian101 2:6b913f93bc0b 47 minYThreshold = 0.2f*uMax;
fabian101 2:6b913f93bc0b 48 } else {motorValueY = -percentageD;
fabian101 2:6b913f93bc0b 49 minYThreshold = 0.2f*dMax;
fabian101 2:6b913f93bc0b 50 }
fabian101 2:6b913f93bc0b 51 // current pulses
fabian101 2:6b913f93bc0b 52 xPulses = pulses.getDistanceX(); // in pulses
fabian101 2:6b913f93bc0b 53 yPulses = pulses.getDistanceY();
fabian101 0:d935ea6f5c25 54
fabian101 1:b9005f2aabf5 55 //edge detection
fabian101 2:6b913f93bc0b 56 if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue
fabian101 2:6b913f93bc0b 57 motorValueX = 0;
fabian101 1:b9005f2aabf5 58 }
fabian101 2:6b913f93bc0b 59 if (xPulses < margin && motorValueX < 0) {
fabian101 2:6b913f93bc0b 60 motorValueX = 0;
fabian101 0:d935ea6f5c25 61 }
fabian101 2:6b913f93bc0b 62 if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue
fabian101 2:6b913f93bc0b 63 motorValueY = 0;
fabian101 0:d935ea6f5c25 64 }
fabian101 2:6b913f93bc0b 65 if (yPulses < margin && motorValueY < 0) {
fabian101 2:6b913f93bc0b 66 motorValueY =0;
fabian101 1:b9005f2aabf5 67 }
fabian101 0:d935ea6f5c25 68
fabian101 1:b9005f2aabf5 69 // Quantize the linear speed request
fabian101 1:b9005f2aabf5 70 // motorValue is a value [-1,1], currently only 0 or 1/-1
fabian101 2:6b913f93bc0b 71 if (motorValueX > maxXValue){motorValueX = maxXValue;} // safety
fabian101 2:6b913f93bc0b 72 if (motorValueX < -maxXValue){motorValueX = -maxXValue;}
fabian101 2:6b913f93bc0b 73 if (motorValueX > minXThreshold && motorValueX < maxXValue){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here
fabian101 2:6b913f93bc0b 74 if (motorValueX < -minXThreshold && motorValueX > -maxXValue){motorValueX = -maxXValue;}
fabian101 0:d935ea6f5c25 75
fabian101 2:6b913f93bc0b 76 if (motorValueY > maxYValue){motorValueY = maxYValue;}
fabian101 2:6b913f93bc0b 77 if (motorValueY < -maxYValue){motorValueY = -maxYValue;}
fabian101 2:6b913f93bc0b 78 if (motorValueY > minYThreshold && motorValueY < maxYValue){motorValueY = maxYValue;}
fabian101 2:6b913f93bc0b 79 if (motorValueY < -minYThreshold && motorValueY > -maxYValue){motorValueY = -maxYValue;}
fabian101 2:6b913f93bc0b 80
fabian101 2:6b913f93bc0b 81
fabian101 2:6b913f93bc0b 82 // current position of pen in Meters
fabian101 2:6b913f93bc0b 83 float currentX = (pulses.getDistanceX()/(8400.0f))*(2*PI)*RmX;
fabian101 2:6b913f93bc0b 84 float currentY = (pulses.getDistanceY()/(8400.0f))*(2*PI)*Rpulley;
fabian101 1:b9005f2aabf5 85
fabian101 2:6b913f93bc0b 86 // upcoming position of pen in Meters
fabian101 2:6b913f93bc0b 87 //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s
fabian101 2:6b913f93bc0b 88 //toY = currentY + motorValueY*maxSpeed*RATE;
fabian101 2:6b913f93bc0b 89
fabian101 2:6b913f93bc0b 90 // distance it should travel for 1 interval in Meters
fabian101 2:6b913f93bc0b 91 float deltaX = motorValueX*maxSpeed*RATE;
fabian101 2:6b913f93bc0b 92 float deltaY = motorValueY*maxSpeed*RATE;
fabian101 1:b9005f2aabf5 93
fabian101 2:6b913f93bc0b 94 // desired velocity in m/s for this interval
fabian101 2:6b913f93bc0b 95 float toVelX = deltaX/RATE;
fabian101 2:6b913f93bc0b 96 float toVelY = deltaY/RATE;
fabian101 2:6b913f93bc0b 97
fabian101 2:6b913f93bc0b 98 // set setpoint of PID controller, this is the thing you want
fabian101 2:6b913f93bc0b 99 X_Controller.setSetPoint(toVelX);
fabian101 2:6b913f93bc0b 100 Y_Controller.setSetPoint(toVelY);
fabian101 0:d935ea6f5c25 101
fabian101 2:6b913f93bc0b 102 // current velocity in M/s, the thing you measure
fabian101 2:6b913f93bc0b 103 // horizontal motor
fabian101 2:6b913f93bc0b 104 float currVelX = (currentX - prevX)/RATE;
fabian101 2:6b913f93bc0b 105 prevX = currentX;
fabian101 2:6b913f93bc0b 106 X_Controller.setProcessValue(currVelX);
fabian101 1:b9005f2aabf5 107
fabian101 2:6b913f93bc0b 108 //vertical motor
fabian101 2:6b913f93bc0b 109 float currVelY = (currentY - prevY)/RATE;
fabian101 2:6b913f93bc0b 110 prevY = currentY;
fabian101 2:6b913f93bc0b 111 Y_Controller.setProcessValue(currVelY);
fabian101 1:b9005f2aabf5 112
fabian101 2:6b913f93bc0b 113 // compute the output FIXME[calibratebutton might break the program]
fabian101 2:6b913f93bc0b 114 float X_MotorOutput = X_Controller.compute();
fabian101 2:6b913f93bc0b 115 if (X_MotorOutput > 0){ // right request
fabian101 2:6b913f93bc0b 116 X_Direction.write(CW);
fabian101 2:6b913f93bc0b 117 X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1
fabian101 2:6b913f93bc0b 118 } else {
fabian101 2:6b913f93bc0b 119 X_Direction.write(!CW); // left request
fabian101 2:6b913f93bc0b 120 X_Magnitude.write(-X_MotorOutput);
fabian101 2:6b913f93bc0b 121 }
fabian101 2:6b913f93bc0b 122
fabian101 2:6b913f93bc0b 123 float Y_MotorOutput = Y_Controller.compute();
fabian101 2:6b913f93bc0b 124 if (Y_MotorOutput > 0){ // up request
fabian101 2:6b913f93bc0b 125 Y_Direction.write(CW);
fabian101 2:6b913f93bc0b 126 Y_Magnitude.write(Y_MotorOutput);
fabian101 2:6b913f93bc0b 127 } else {
fabian101 2:6b913f93bc0b 128 Y_Direction.write(!CW); // down request
fabian101 2:6b913f93bc0b 129 Y_Magnitude.write(-Y_MotorOutput);
fabian101 2:6b913f93bc0b 130 }
fabian101 2:6b913f93bc0b 131 }
fabian101 0:d935ea6f5c25 132
fabian101 0:d935ea6f5c25 133 // output to hidscope
fabian101 2:6b913f93bc0b 134
fabian101 0:d935ea6f5c25 135 flag = false; // only output to motor with RATE
fabian101 2:6b913f93bc0b 136 i = 1;
fabian101 0:d935ea6f5c25 137 }
fabian101 0:d935ea6f5c25 138
fabian101 0:d935ea6f5c25 139 }
fabian101 0:d935ea6f5c25 140 }
fabian101 0:d935ea6f5c25 141
fabian101 0:d935ea6f5c25 142 void setFlag() {
fabian101 0:d935ea6f5c25 143 flag = true;
fabian101 2:6b913f93bc0b 144 i++;
fabian101 1:b9005f2aabf5 145 }
fabian101 1:b9005f2aabf5 146
fabian101 1:b9005f2aabf5 147 void calibrate() {
fabian101 2:6b913f93bc0b 148 motorControl.detach();
fabian101 2:6b913f93bc0b 149 calibrateFlag = false;
fabian101 2:6b913f93bc0b 150 // GO-TO pos (0,0)
fabian101 2:6b913f93bc0b 151 while ((pulses.getDistanceX() > margin) && (pulses.getDistanceY() > margin)){
fabian101 2:6b913f93bc0b 152 if (pulses.getDistanceX() > margin){
fabian101 1:b9005f2aabf5 153 // go direction LEFT
fabian101 2:6b913f93bc0b 154 X_Direction.write(!CW);
fabian101 2:6b913f93bc0b 155 X_Magnitude.write(1.0);
fabian101 2:6b913f93bc0b 156 } else { //motormagX 0
fabian101 2:6b913f93bc0b 157 X_Direction.write(CW);
fabian101 2:6b913f93bc0b 158 X_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 159 }
fabian101 2:6b913f93bc0b 160 if (pulses.getDistanceY() > margin){
fabian101 1:b9005f2aabf5 161 // go direction DOWN
fabian101 2:6b913f93bc0b 162 Y_Direction.write(!CW);
fabian101 2:6b913f93bc0b 163 Y_Magnitude.write(1.0);
fabian101 2:6b913f93bc0b 164 } else { //motormagY 0
fabian101 2:6b913f93bc0b 165 Y_Direction.write(CW);
fabian101 2:6b913f93bc0b 166 Y_Magnitude.write(0.0);
fabian101 2:6b913f93bc0b 167 }
fabian101 1:b9005f2aabf5 168 // problem with the interrupt button, after calibration the program should re-start the run function instead of going further at the point it left
fabian101 1:b9005f2aabf5 169 // this will cause 1 iteration to be absolutely wrong -> consequences?
fabian101 2:6b913f93bc0b 170
fabian101 1:b9005f2aabf5 171 }
fabian101 2:6b913f93bc0b 172 prevX = getDistanceX();
fabian101 2:6b913f93bc0b 173 prevY = getDistanceY();
fabian101 2:6b913f93bc0b 174 r=0; l=0; u=0; d=0;
fabian101 2:6b913f93bc0b 175 i=0;
fabian101 2:6b913f93bc0b 176 motorControl.attach(&setFlag, RATE);
fabian101 1:b9005f2aabf5 177 }
fabian101 1:b9005f2aabf5 178
fabian101 1:b9005f2aabf5 179 void calibratePower(){
fabian101 1:b9005f2aabf5 180 while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME
fabian101 1:b9005f2aabf5 181 // do this for 5 sec
fabian101 1:b9005f2aabf5 182 double rCal = signal.readValue(1);
fabian101 1:b9005f2aabf5 183 if ( rCal > rMax){
fabian101 1:b9005f2aabf5 184 rMax = rCal;
fabian101 1:b9005f2aabf5 185 }
fabian101 1:b9005f2aabf5 186 double lCal = signal.readValue(2);
fabian101 1:b9005f2aabf5 187 if ( lCal > lMax){
fabian101 1:b9005f2aabf5 188 lMax = lCal;
fabian101 1:b9005f2aabf5 189 }
fabian101 1:b9005f2aabf5 190 double uCal = signal.readValue(3);
fabian101 1:b9005f2aabf5 191 if ( uCal > uMax){
fabian101 1:b9005f2aabf5 192 uMax = uCal;
fabian101 1:b9005f2aabf5 193 }
fabian101 1:b9005f2aabf5 194 double dCal = signal.readValue(4);
fabian101 1:b9005f2aabf5 195 if ( dCal > dMax){
fabian101 1:b9005f2aabf5 196 dMax = dCal;
fabian101 1:b9005f2aabf5 197 }
fabian101 1:b9005f2aabf5 198 }
fabian101 1:b9005f2aabf5 199 return;
fabian101 1:b9005f2aabf5 200 }