Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_control_v3 by
Controller.cpp
- Committer:
- fabian101
- Date:
- 2016-10-28
- Revision:
- 4:bfdcf3da7cff
- Parent:
- 2:6b913f93bc0b
- Child:
- 6:a4440eec3652
File content as of revision 4:bfdcf3da7cff:
#include "Controller.h" int main() { //pc.baud(115200); //pc.printf("startup: \n\r"); //calibrateButton.rise(&calibrate); initMotors(); initControllers(); //calibratePower(); // store max amplitude each emg signal while(true) { //calibrate(); // calibrate position while(!button){} // button not pressed is true, this is here to make sure that the it will only go run if you have let go of the button and calibrated run(); } } void run() { motorControl.attach(&setUpdate_flag, RATE); // rate =0.001 while(button) // button not presseed { //pc.printf("run: %d\n\r", counter); if(update_flag) { led = !led; //counter++; // returns filtered value r += signal.readValue(1)/SAMPLES_PER_AVERAGE; float r1= signal.readValue(1); l += signal.readValue(2)/SAMPLES_PER_AVERAGE; u += signal.readValue(3)/SAMPLES_PER_AVERAGE; d += signal.readValue(4)/SAMPLES_PER_AVERAGE; scope.set(0,r); scope.set(1,r1); scope.send(); i++; update_flag = false; } if(i==SAMPLES_PER_AVERAGE) // the filtering works on a frequency which is 10 times higher, { //counter_motor++; //scope.set(0,counter_motor); // emg1 // calculate r,l,u,d in a % of max double percentageR = r/rMax; double percentageL = l/lMax; double percentageU = u/uMax; double percentageD = d/dMax; double minXThreshold; double minYThreshold; r=0; l=0; u=0; d=0; // ignore the weaker signal,motorValue calculation, threshold set if ( percentageR > percentageL) { // horizontal speed request motorValueX = percentageR; minXThreshold = 0.2f*rMax; // set the threshold for quantizer } else { motorValueX = -percentageL; minXThreshold = 0.2f*lMax; } if (percentageU > percentageD) // vertical speed request { motorValueY = percentageU; minYThreshold = 0.2f*uMax; } else { motorValueY = -percentageD; minYThreshold = 0.2f*dMax; } // current pulses xPulses = X_Motor.getPulses(); yPulses = Y_Motor.getPulses(); //edge detection if (xPulses > (x_max-margin) && motorValueX > 0) { // right should correspond to positive motorvalue motorValueX = 0; } if (xPulses < margin && motorValueX < 0) { motorValueX = 0; } if (yPulses > (y_max-margin) && motorValueY > 0) { // up should correspond to positive motorvalue motorValueY = 0; } if (yPulses < margin && motorValueY < 0) { motorValueY =0; } // Quantize the linear speed request // motorValue is a value [-1,1], currently only 0 or 1/-1 if (motorValueX > maxXValue){motorValueX = maxXValue;} // safety if (motorValueX < -maxXValue){motorValueX = -maxXValue;} if (motorValueX > minXThreshold && motorValueX < maxXValue){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here if (motorValueX < -minXThreshold && motorValueX > -maxXValue){motorValueX = -maxXValue;} if (motorValueY > maxYValue){motorValueY = maxYValue;} if (motorValueY < -maxYValue){motorValueY = -maxYValue;} if (motorValueY > minYThreshold && motorValueY < maxYValue){motorValueY = maxYValue;} if (motorValueY < -minYThreshold && motorValueY > -maxYValue){motorValueY = -maxYValue;} // current position of pen in Meters float currentX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; float currentY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; // upcoming position of pen in Meters //toX = currentX + motorValueX*maxSpeed*RATE; // speed is in m/s rate is in s //toY = currentY + motorValueY*maxSpeed*RATE; // distance it should travel for 1 interval in Meters float deltaX = motorValueX*maxSpeed*RATE; float deltaY = motorValueY*maxSpeed*RATE; // desired velocity in m/s for this interval float toVelX = deltaX/RATE; float toVelY = deltaY/RATE; // set setpoint of PID controller, this is the thing you want X_Controller.setSetPoint(toVelX); Y_Controller.setSetPoint(toVelY); // current velocity in M/s, the thing you measure // horizontal motor float currVelX = (currentX - prevX)/RATE; prevX = currentX; X_Controller.setProcessValue(currVelX); //vertical motor float currVelY = (currentY - prevY)/RATE; prevY = currentY; Y_Controller.setProcessValue(currVelY); // compute the output float X_MotorOutput = X_Controller.compute(); if (X_MotorOutput > 0) { // right request X_Direction.write(CW); X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 } else { X_Direction.write(!CW); // left request X_Magnitude.write(-X_MotorOutput); //motor output will be negative and you need to write a pos value } float Y_MotorOutput = Y_Controller.compute(); if (Y_MotorOutput > 0) { // up request Y_Direction.write(CW); Y_Magnitude.write(Y_MotorOutput); } else { Y_Direction.write(!CW); // down request Y_Magnitude.write(-Y_MotorOutput); } i = 0; } } } void setUpdate_flag() { update_flag = true; } void calibrate() { motorControl.detach(); // GO-TO pos (0,0) while ((X_Motor.getPulses() > margin) && (Y_Motor.getPulses() > margin)){ if (X_Motor.getPulses() > margin){ // go direction LEFT X_Direction.write(!CW); X_Magnitude.write(1.0); // WRONG, reqVel must be 1cm/s here } else { //motormagX 0 X_Direction.write(CW); X_Magnitude.write(0.0); } if (Y_Motor.getPulses() > margin){ // go direction DOWN Y_Direction.write(!CW); Y_Magnitude.write(1.0); } else { //motormagY 0 Y_Direction.write(CW); Y_Magnitude.write(0.0); } } prevX = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX; prevY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley; r=0; l=0; u=0; d=0; i=0; } void calibratePower(){ while((clock()/CLOCKS_PER_SEC) < 5) { // FIXME // do this for 5 sec float rCal = signal.readValue(1); if ( rCal > rMax) { rMax = rCal; } float lCal = signal.readValue(2); if ( lCal > lMax) { lMax = lCal; } float uCal = signal.readValue(3); if ( uCal > uMax) { uMax = uCal; } float dCal = signal.readValue(4); if ( dCal > dMax) { dMax = dCal; } } return; }