Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_control_v3 by
Revision 8:82e38f4aa690, committed 2016-11-07
- Comitter:
- AlexGroup19
- Date:
- Mon Nov 07 11:29:28 2016 +0000
- Parent:
- 7:2ab4bdd1f998
- Commit message:
- Group19-Biorobotics
Changed in this revision
Controller.cpp | Show annotated file Show diff for this revision Revisions of this file |
Controller.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/Controller.cpp Wed Nov 02 15:40:15 2016 +0000 +++ b/Controller.cpp Mon Nov 07 11:29:28 2016 +0000 @@ -16,7 +16,6 @@ void run() { - //pc.printf("ik ga nu runnen G \n\r"); motorControl.attach(&setUpdate_flag, RATE); // rate =0.001 while(button) // button not presseed { @@ -24,47 +23,42 @@ { // 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); + l = signal.readValue(2); + u = signal.readValue(3); + d = signal.readValue(4); - //u = signal.readValue(3); - // d = signal.readValue(4); + pc.printf("r : %f \n\r", r); + pc.printf("l : %f \n\r", l); // 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 @@ -78,27 +72,31 @@ { // up should correspond to positive motorvalue motorValueY = 0; } - if ((yPulses < margin) && (motorValueY < -sensitivityFactor)) + if ((yPulses < margin) && (motorValueY < ((-1) * sensitivityFactor))) { - 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 - 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;} + // motorValue is a value [-1,1], currently only 0 or 1/-1 + + //pc.printf("X_MotorValue : %f \n\r", motorValueX); + if (motorValueX >= maxXValue){motorValueX = maxXValue;} // boundary for the motor value + else if (motorValueX <= -maxXValue){motorValueX = -maxXValue;} + else if ((motorValueX >= sensitivityFactor) && (motorValueX < maxXValue)){motorValueX = maxXValue;} // if threshold is met go a fixed speed%, we could 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); + else if (motorValueY <= -maxYValue){motorValueY = -maxYValue;} + else if ((motorValueY >= sensitivityFactor) && (motorValueY < maxYValue)){motorValueY = maxYValue;} + else if ((motorValueY <= -sensitivityFactor) && (motorValueY > -maxYValue)){motorValueY = -maxYValue;} + else {motorValueY = 0;} + + + // 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 @@ -107,11 +105,11 @@ // 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 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); @@ -120,51 +118,78 @@ // 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 + float X_MotorOutput = 9 * X_Controller.compute(); + //pc.printf("X_MotorOutput : %f \n\r", X_MotorOutput); + + if (X_MotorOutput > maxSpeed) // { // right request X_Direction.write(CW); - X_Magnitude.write(X_MotorOutput); // must be value between 0 and 1 + X_Magnitude.write(maxSpeed); // } - else if (X_MotorOutput <= -X_frictionTrheshold) + else if (X_MotorOutput <= -maxSpeed) { 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 + X_Magnitude.write(maxSpeed); //motor output will be negative and you need to write a pos value + } + else if ((X_MotorOutput < maxSpeed) && (X_MotorOutput > sensitivityFactor)) + { + X_Magnitude.write(X_MotorOutput); + X_Direction.write(CW); } - else // else the internal friction is not compensated + + else if ((X_MotorOutput > -maxSpeed) && (X_MotorOutput < -sensitivityFactor)) + { + X_Magnitude.write(X_MotorOutput); + X_Direction.write(!CW); + } + else { X_Magnitude.write(0); - X_Direction.write(CW); + } + + + float Y_MotorOutput = 9 * Y_Controller.compute(); + if (Y_MotorOutput > maxSpeed) // + { // right request + Y_Direction.write(CW); + Y_Magnitude.write(maxSpeed); // + } + else if (Y_MotorOutput <= -maxSpeed) + { + Y_Direction.write(!CW); // left request + Y_Magnitude.write(maxSpeed); //motor output will be negative and you need to write a pos value + } + else if ((Y_MotorOutput < maxSpeed) && (Y_MotorOutput > sensitivityFactor)) + { + Y_Magnitude.write(Y_MotorOutput); + Y_Direction.write(CW); } - float Y_MotorOutput = Y_Controller.compute(); - if (Y_MotorOutput >= Y_frictionTrheshold) - { // up request - Y_Direction.write(CW); + + else if ((Y_MotorOutput > -maxSpeed) && (Y_MotorOutput < -sensitivityFactor)) + { Y_Magnitude.write(Y_MotorOutput); - } - else if (Y_MotorOutput <= -Y_frictionTrheshold) - { - Y_Direction.write(!CW); // down request - Y_Magnitude.write(fabs(Y_MotorOutput)); + Y_Direction.write(!CW); } - else + + else { Y_Magnitude.write(0); - Y_Direction.write(CW); } + update_flag = false; + } } motorControl.detach(); @@ -180,11 +205,12 @@ 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 + X_Magnitude.write(calibrationSpeed); // 8 RPM with no load, maybe compensate for internal friction } else { //motormagX 0 @@ -195,7 +221,7 @@ { // go direction DOWN Y_Direction.write(!CW); - Y_Magnitude.write(0.4); + Y_Magnitude.write(calibrationSpeed); } else { //motormagY 0 @@ -225,7 +251,7 @@ { lMax = lCal; } - /* + float uCal = signal.readValue(3); if ( uCal > uMax) { @@ -236,10 +262,7 @@ { dMax = dCal; } - */ - counter++; - //pc.printf("rCal: %f \n\r", rCal); - + counter++; update_flag = false; } } @@ -248,6 +271,4 @@ pc.printf("lMax : %f\n\r", lMax); motorControl.detach(); led = true; - //pc.printf("ik moet hier led weer uitzetten \n\r"); - }
--- a/Controller.h Wed Nov 02 15:40:15 2016 +0000 +++ b/Controller.h Mon Nov 07 11:29:28 2016 +0000 @@ -7,7 +7,7 @@ //HIDScope scope(2); int xPulses = 0; // pulsecount -const int x_max = 30068;// 30068; +const int x_max = 31068;// 30068; int yPulses = 0; const int y_max = 15003; int prevXpulses = 0; @@ -24,6 +24,7 @@ float X_frictionTrheshold = 0.04; float Y_frictionTrheshold = 0.04; float sensitivityFactor = 0.2; // determines treshold value for contraction, value between 0 and 1 +float calibrationSpeed = 0.4; // [0,1], determines how fast the system returns to (0,0) when the button is pressed const float R1 = 0.085; // radius of big driven gear in m const float RmX = 0.012; // radius of small drive gear in m @@ -55,8 +56,8 @@ void initControllers(); void initMotors(); -const float KcX = 0.0358; // THIS IS THE LIMIT FOR STABILITY -const float KcY = 0.366; +const float KcX = 0.02058; // THIS IS THE LIMIT FOR STABILITY +const float KcY = 0.02066; const float Ti = 0.0; const float Td = 0.0; const float RATE = 0.002; // rate = interval motors will be updateed