biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Controller.cpp
- Committer:
- fabian101
- Date:
- 2016-11-02
- Revision:
- 7:2ab4bdd1f998
- Parent:
- 6:a4440eec3652
File content as of revision 7:2ab4bdd1f998:
#include "Controller.h" int main() { pc.baud(115200); 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() { //pc.printf("ik ga nu runnen G \n\r"); motorControl.attach(&setUpdate_flag, RATE); // rate =0.001 while(button) // button not presseed { if(update_flag) // { // returns filtered value r = signal.readValue(1); l = signal.readValue(2); //pc.printf("rechts is : %f\n\r", r); //pc.printf("links is : %f\n\r", l); //u = signal.readValue(3); // d = signal.readValue(4); // calculate r,l,u,d relative to the max [0,1] percentageR = r/rMax; percentageL = l/lMax; percentageU = u/uMax; percentageD = d/dMax; //scope.set(0,percentageR); //scope.set(1,percentageL); //scope.send(); //pc.printf("wat is percentageL : %f \n\r", percentageL); // ignore the weaker signal,motorValue calculation, threshold set if ( percentageR > percentageL) { // horizontal speed request motorValueX = percentageR; //pc.printf("rechts > links : %f\n\r", motorValueX); } else { motorValueX = -percentageL; //pc.printf("rechts < links : %f\n\r", motorValueX); } if (percentageU > percentageD) // vertical speed request { motorValueY = percentageU; } else { motorValueY = -percentageD; } //pc.printf("motorValueX : %f\n\r", motorValueX); // current pulses xPulses = X_Motor.getPulses(); // pc.printf("curentpulses : %f \n\r", xPulses); yPulses = Y_Motor.getPulses(); //edge detection if ((xPulses > (x_max-margin)) && (motorValueX > sensitivityFactor)) // (x_max-margin { // right should correspond to positive motorvalue motorValueX = 0; } if ((xPulses < margin) && (motorValueX < (-1 * sensitivityFactor))) { motorValueX = 0; } if ((yPulses > (y_max-margin)) && (motorValueY > sensitivityFactor)) { // up should correspond to positive motorvalue motorValueY = 0; } if ((yPulses < margin) && (motorValueY < -sensitivityFactor)) { 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 else if (motorValueX <= -maxXValue){motorValueX = -maxXValue;} else if ((motorValueX >= sensitivityFactor) && (motorValueX < maxXValue)){motorValueX = maxXValue;} // if threshold is met pwm is 100%, maybe quantize here else if ((motorValueX <= -sensitivityFactor) && (motorValueX > -maxXValue)){motorValueX = -maxXValue;} else {motorValueX = 0;} if (motorValueY >= maxYValue){motorValueY = maxYValue;} else if (motorValueY <= -maxYValue){motorValueY = -maxYValue;} else if ((motorValueY >= sensitivityFactor) && (motorValueY <= maxYValue)){motorValueY = maxYValue;} else if ((motorValueY <= -sensitivityFactor) && (motorValueY >= -maxYValue)){motorValueY = -maxYValue;} else {motorValueX = 0;} //pc.printf("dit moet nu value hebben als je aanspant : %f \n\r", motorValueX); // current position of pen in Meters float currentX = (xPulses/(8400.0f))*(2*PI)*RmX; float currentY = (yPulses/(8400.0f))*(2*PI)*Rpulley; //pc.printf("currentX : %f \n\r", currentX); // 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; //pc.printf("deltaX : %f \n\r", deltaX); // desired velocity in m/s for this interval float toVelX = deltaX/RATE; float toVelY = deltaY/RATE; //pc.printf("toVelX : %f \n\r", toVelX); // 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; //pc.printf("currVe;X : %f \n\r", currVelX); X_Controller.setProcessValue(currVelX); //vertical motor float currVelY = (currentY - prevY)/RATE; prevY = currentY; Y_Controller.setProcessValue(currVelY); // compute the output float X_MotorOutput = 10*(X_Controller.compute()); pc.printf("X_MotorOutput : %f \n\r", X_MotorOutput); if (X_MotorOutput >= X_frictionTrheshold) // hardcoded threshold internal friction { // right request X_Direction.write(CW); X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 } else if (X_MotorOutput <= -X_frictionTrheshold) { X_Direction.write(!CW); // left request X_Magnitude.write(fabs(X_MotorOutput)); //motor output will be negative and you need to write a pos value } else // else the internal friction is not compensated { X_Magnitude.write(0); X_Direction.write(CW); } float Y_MotorOutput = Y_Controller.compute(); if (Y_MotorOutput >= Y_frictionTrheshold) { // up request Y_Direction.write(CW); Y_Magnitude.write(Y_MotorOutput); } else if (Y_MotorOutput <= -Y_frictionTrheshold) { Y_Direction.write(!CW); // down request Y_Magnitude.write(fabs(Y_MotorOutput)); } else { Y_Magnitude.write(0); Y_Direction.write(CW); } update_flag = false; } } motorControl.detach(); } void setUpdate_flag() { update_flag = true; } void calibrate() { // GO-TO pos (0,0) while ((X_Motor.getPulses() > margin) || (Y_Motor.getPulses() > margin)){ float pulses = X_Motor.getPulses(); printf("pulsesX : %f\n\r", pulses); if (X_Motor.getPulses() > margin) { // go direction LEFT X_Direction.write(!CW); X_Magnitude.write(0.4); // 8 RPM with no load, maybe compensate for internal friction } 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(0.4); } 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; } void calibratePower(){ motorControl.attach(&setUpdate_flag, RATE); led = false; // contract now, false = on while (counter < 3000) // get 3000 samples for the max { if(update_flag) { 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; } */ counter++; //pc.printf("rCal: %f \n\r", rCal); update_flag = false; } } counter = 0; pc.printf("rMax : %f\n\r", rMax); pc.printf("lMax : %f\n\r", lMax); motorControl.detach(); led = true; //pc.printf("ik moet hier led weer uitzetten \n\r"); }