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@1:b9005f2aabf5, 2016-10-21 (annotated)

Committer:
fabian101
Date:
Fri Oct 21 21:32:20 2016 +0000
Revision:
1:b9005f2aabf5
Parent:
0:d935ea6f5c25
Child:
2:6b913f93bc0b
21-10 11:32

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 0:d935ea6f5c25 6 motorControl.attach(&setFlag, RATE);
fabian101 1:b9005f2aabf5 7 initMotors();
fabian101 1:b9005f2aabf5 8 initControllers();
fabian101 1:b9005f2aabf5 9 calibratePower(); // store max amplitude each emg signal
fabian101 0:d935ea6f5c25 10 run();
fabian101 0:d935ea6f5c25 11 }
fabian101 0:d935ea6f5c25 12
fabian101 0:d935ea6f5c25 13 void run() {
fabian101 0:d935ea6f5c25 14 while(true){
fabian101 0:d935ea6f5c25 15 double r = signal.readValue(1); // returns filtered value
fabian101 0:d935ea6f5c25 16 double l = signal.readValue(2);
fabian101 0:d935ea6f5c25 17 double u = signal.readValue(3);
fabian101 0:d935ea6f5c25 18 double d = signal.readValue(4);
fabian101 0:d935ea6f5c25 19
fabian101 0:d935ea6f5c25 20 // calculate r,l,u,d in a % of max
fabian101 0:d935ea6f5c25 21 double percentageR = r/rMax;
fabian101 0:d935ea6f5c25 22 double percentageL = l/lMax;
fabian101 0:d935ea6f5c25 23 double percentageU = u/uMax;
fabian101 0:d935ea6f5c25 24 double percentageD = d/dMax;
fabian101 0:d935ea6f5c25 25
fabian101 0:d935ea6f5c25 26
fabian101 1:b9005f2aabf5 27 if (flag) { // setting the flag here will cause the edge detection to be ignored if the flag is false for some reason
fabian101 1:b9005f2aabf5 28 // ignore the weaker signal,motorValue calculation, threshold set
fabian101 1:b9005f2aabf5 29 if ( percentageR > percentageL){ // horizontal speed request
fabian101 0:d935ea6f5c25 30 motorValueHor = percentageR;
fabian101 1:b9005f2aabf5 31 minHorThreshold = 0.2*rMax; // set the threshold
fabian101 1:b9005f2aabf5 32 } else {motorValueHor = -percentageL;
fabian101 1:b9005f2aabf5 33 minHorThreshold = 0.2*lMax;
fabian101 1:b9005f2aabf5 34 }
fabian101 1:b9005f2aabf5 35 if (percentageU > percentageD) { // vertical speed request
fabian101 0:d935ea6f5c25 36 motorValueVer = percentageU;
fabian101 1:b9005f2aabf5 37 minVerThreshold = 0.2*uMax;
fabian101 1:b9005f2aabf5 38 } else {motorValueVer = -percentageD;
fabian101 1:b9005f2aabf5 39 minVerThreshold = 0.2*uMax;
fabian101 1:b9005f2aabf5 40 }
fabian101 0:d935ea6f5c25 41 // current pulses and prevpulses calculated
fabian101 0:d935ea6f5c25 42 x += (pulses.getDistanceHor() - prevX); // in pulses
fabian101 0:d935ea6f5c25 43 prevX = x;
fabian101 0:d935ea6f5c25 44 y += (pulses.getDistanceVer() - prevY);
fabian101 0:d935ea6f5c25 45 prevY = y;
fabian101 0:d935ea6f5c25 46
fabian101 1:b9005f2aabf5 47 //edge detection
fabian101 1:b9005f2aabf5 48 if (x > x_max && motorValueHor > 0) { // right should correspond to positive motorvalue
fabian101 1:b9005f2aabf5 49 motorValueHor = 0;
fabian101 1:b9005f2aabf5 50 }
fabian101 1:b9005f2aabf5 51 if (x < 0 && motorValueHor < 0) { // carefulwith 0 pulses, maybe implement margin
fabian101 1:b9005f2aabf5 52 motorValueHor = 0;
fabian101 0:d935ea6f5c25 53 }
fabian101 1:b9005f2aabf5 54 if (y > y_max && motorValueVer > 0) { // up should correspond to positive motorvalue
fabian101 1:b9005f2aabf5 55 motorValueVer = 0;
fabian101 0:d935ea6f5c25 56 }
fabian101 1:b9005f2aabf5 57 if (y < 0 && motorValueVer < 0) {
fabian101 1:b9005f2aabf5 58 motorValueVer =0;
fabian101 1:b9005f2aabf5 59 }
fabian101 0:d935ea6f5c25 60
fabian101 1:b9005f2aabf5 61 // Quantize the linear speed request
fabian101 1:b9005f2aabf5 62 // motorValue is a value [-1,1], currently only 0 or 1/-1
fabian101 1:b9005f2aabf5 63 if (motorValueHor > maxHorValue){motorValueHor = maxHorValue;} // safety
fabian101 1:b9005f2aabf5 64 if (motorValueHor < -maxHorValue){motorValueHor = -maxHorValue;}
fabian101 1:b9005f2aabf5 65 if (motorValueHor > minHorThreshold && motorValueHor < maxHorValue){motorValueHor = maxHorValue;} // if threshold is met pwm is 100%, maybe quantize here
fabian101 1:b9005f2aabf5 66 if (motorValueHor < -minHorThreshold && motorValueHor > -maxHorValue){motorValueHor = -maxHorValue;}
fabian101 0:d935ea6f5c25 67
fabian101 1:b9005f2aabf5 68 if (motorValueVer > maxVerValue){motorValueVer = maxVerValue;}
fabian101 1:b9005f2aabf5 69 if (motorValueVer < -maxVerValue){motorValueVer = -maxVerValue;}
fabian101 1:b9005f2aabf5 70 if (motorValueVer > minVerThreshold && motorValueVer < maxVerValue){motorValueVer = maxVerValue;}
fabian101 1:b9005f2aabf5 71 if (motorValueVer < -minVerThreshold && motorValueVer > -maxVerValue){motorValueVer = -maxVerValue;}
fabian101 1:b9005f2aabf5 72
fabian101 1:b9005f2aabf5 73 //current position of pen in degrees
fabian101 1:b9005f2aabf5 74 currentXangle = 360.0f*(pulses.getDistanceHor()/8400);
fabian101 1:b9005f2aabf5 75 currentYangle = 360.0f*(pulses.getDistanceVer()/8400);
fabian101 1:b9005f2aabf5 76
fabian101 1:b9005f2aabf5 77 // current position of pen in cm
fabian101 1:b9005f2aabf5 78 currentX = (360.0f/(2*PI))*(pulses.getDistanceHor()/8400)*(RmHor/R1); // radiants*revolutions*gearratio is x pos in rad IS RAD EQUAL TO CM?????????
fabian101 1:b9005f2aabf5 79 currentY= (360.0f/(2*PI))*(pulses.getDistanceVer()/8400); // radiants*revolutions 1:1 ratio??
fabian101 0:d935ea6f5c25 80
fabian101 1:b9005f2aabf5 81 // desired position of pen in cm
fabian101 1:b9005f2aabf5 82 toX = currentX + motorValueHor*maxSpeed*RATE;
fabian101 1:b9005f2aabf5 83 toY = currentY + motorValueVer*maxSpeed*RATE;
fabian101 1:b9005f2aabf5 84
fabian101 1:b9005f2aabf5 85 // desired angle of pen in degrees
fabian101 1:b9005f2aabf5 86 toangleX = ((toX-currentX)*(R1/RmHor))*(360.0f/(2*PI));
fabian101 1:b9005f2aabf5 87 toangleY = (toX-currentX)*(360.0f/(2*PI));
fabian101 1:b9005f2aabf5 88
fabian101 1:b9005f2aabf5 89 //requested velocities in degrees/sec
fabian101 1:b9005f2aabf5 90 Xvelrequest = (toangleX - currentXangle) / RATE;
fabian101 1:b9005f2aabf5 91 Yvelrequest = (toangleY - currentYangle) / RATE;
fabian101 1:b9005f2aabf5 92
fabian101 1:b9005f2aabf5 93 // set setpoint of PID controller here with velrequest, what are the input limits???
fabian101 0:d935ea6f5c25 94
fabian101 0:d935ea6f5c25 95 // Calculate current rotational velocity
fabian101 0:d935ea6f5c25 96
fabian101 1:b9005f2aabf5 97 // give the PID controller the processvariable
fabian101 0:d935ea6f5c25 98
fabian101 0:d935ea6f5c25 99 // compute PID
fabian101 0:d935ea6f5c25 100
fabian101 0:d935ea6f5c25 101 // output to motor
fabian101 0:d935ea6f5c25 102
fabian101 0:d935ea6f5c25 103 // output to hidscope
fabian101 0:d935ea6f5c25 104 flag = false; // only output to motor with RATE
fabian101 0:d935ea6f5c25 105 }
fabian101 0:d935ea6f5c25 106
fabian101 0:d935ea6f5c25 107 }
fabian101 0:d935ea6f5c25 108 }
fabian101 0:d935ea6f5c25 109
fabian101 0:d935ea6f5c25 110 void setFlag() {
fabian101 0:d935ea6f5c25 111 flag = true;
fabian101 0:d935ea6f5c25 112 }
fabian101 0:d935ea6f5c25 113
fabian101 0:d935ea6f5c25 114 void emergencyExit() {
fabian101 0:d935ea6f5c25 115 calibrate();
fabian101 0:d935ea6f5c25 116 run();
fabian101 1:b9005f2aabf5 117 }
fabian101 1:b9005f2aabf5 118
fabian101 1:b9005f2aabf5 119 void calibrate() {
fabian101 1:b9005f2aabf5 120 while ((pulses.getDistanceHor() > margin) && (pulses.getDistanceVer() > margin)){
fabian101 1:b9005f2aabf5 121 if (pulses.getDistanceHor() > margin){
fabian101 1:b9005f2aabf5 122 // go direction LEFT
fabian101 1:b9005f2aabf5 123 }
fabian101 1:b9005f2aabf5 124 else { //motormaghor 0
fabian101 1:b9005f2aabf5 125 }
fabian101 1:b9005f2aabf5 126 if (pulses.getDistanceVer() > margin){
fabian101 1:b9005f2aabf5 127 // go direction DOWN
fabian101 1:b9005f2aabf5 128 }
fabian101 1:b9005f2aabf5 129 else { //motormagver 0}
fabian101 1:b9005f2aabf5 130 }
fabian101 1:b9005f2aabf5 131 // GO-TO pos (0,0)
fabian101 1:b9005f2aabf5 132 // pulses can only be 0 or positive so reverse the direction until 0 pulses is reached
fabian101 1:b9005f2aabf5 133 // 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 134 // this will cause 1 iteration to be absolutely wrong -> consequences?
fabian101 1:b9005f2aabf5 135 }
fabian101 1:b9005f2aabf5 136 }
fabian101 1:b9005f2aabf5 137
fabian101 1:b9005f2aabf5 138 void calibratePower(){
fabian101 1:b9005f2aabf5 139 while((clock()/CLOCKS_PER_SEC) < 5){ // FIXME
fabian101 1:b9005f2aabf5 140 // do this for 5 sec
fabian101 1:b9005f2aabf5 141 double rCal = signal.readValue(1);
fabian101 1:b9005f2aabf5 142 if ( rCal > rMax){
fabian101 1:b9005f2aabf5 143 rMax = rCal;
fabian101 1:b9005f2aabf5 144 }
fabian101 1:b9005f2aabf5 145 double lCal = signal.readValue(2);
fabian101 1:b9005f2aabf5 146 if ( lCal > lMax){
fabian101 1:b9005f2aabf5 147 lMax = lCal;
fabian101 1:b9005f2aabf5 148 }
fabian101 1:b9005f2aabf5 149 double uCal = signal.readValue(3);
fabian101 1:b9005f2aabf5 150 if ( uCal > uMax){
fabian101 1:b9005f2aabf5 151 uMax = uCal;
fabian101 1:b9005f2aabf5 152 }
fabian101 1:b9005f2aabf5 153 double dCal = signal.readValue(4);
fabian101 1:b9005f2aabf5 154 if ( dCal > dMax){
fabian101 1:b9005f2aabf5 155 dMax = dCal;
fabian101 1:b9005f2aabf5 156 }
fabian101 1:b9005f2aabf5 157 }
fabian101 1:b9005f2aabf5 158 return;
fabian101 1:b9005f2aabf5 159 }