Irnti Nousi
Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of
Robot_control_v3
by 
Fabian van Hummel
Diff: Controller.cpp
- Revision:
- 6:a4440eec3652
- Parent:
- 4:bfdcf3da7cff
- Child:
- 8:82e38f4aa690
--- a/Controller.cpp Fri Oct 28 12:50:44 2016 +0000
+++ b/Controller.cpp Wed Nov 02 15:39:10 2016 +0000
@@ -1,16 +1,14 @@
#include "Controller.h"
int main() {
- //pc.baud(115200);
- //pc.printf("startup: \n\r");
- //calibrateButton.rise(&calibrate);
-
+ pc.baud(115200);
initMotors();
initControllers();
- //calibratePower(); // store max amplitude each emg signal
+ calibratePower(); // store max amplitude each emg signal
+
while(true)
{
- //calibrate(); // calibrate position
+ 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();
}
@@ -18,105 +16,89 @@
void run()
{
-
+ //pc.printf("ik ga nu runnen G \n\r");
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++;
+ if(update_flag) //
+ {
// 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;
+ r = signal.readValue(1);
+ l = signal.readValue(2);
+ //pc.printf("rechts is : %f\n\r", r);
+ //pc.printf("links is : %f\n\r", l);
- 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;
-
+ //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;
- minXThreshold = 0.2f*rMax; // set the threshold for quantizer
+ //pc.printf("rechts > links : %f\n\r", motorValueX);
}
else
{
motorValueX = -percentageL;
- minXThreshold = 0.2f*lMax;
+ //pc.printf("rechts < links : %f\n\r", motorValueX);
}
if (percentageU > percentageD) // vertical speed request
{
motorValueY = percentageU;
- minYThreshold = 0.2f*uMax;
}
else
{
motorValueY = -percentageD;
- minYThreshold = 0.2f*dMax;
}
+
+ //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 > 0)
+ if ((xPulses > (x_max-margin)) && (motorValueX > sensitivityFactor)) // (x_max-margin
{ // right should correspond to positive motorvalue
motorValueX = 0;
}
- if (xPulses < margin && motorValueX < 0)
+ if ((xPulses < margin) && (motorValueX < (-1 * sensitivityFactor)))
{
motorValueX = 0;
}
- if (yPulses > (y_max-margin) && motorValueY > 0)
+ if ((yPulses > (y_max-margin)) && (motorValueY > sensitivityFactor))
{ // up should correspond to positive motorvalue
motorValueY = 0;
}
- if (yPulses < margin && 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
- 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;}
-
+ // 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 = (X_Motor.getPulses()/(8400.0f))*(2*PI)*RmX;
- float currentY = (X_Motor.getPulses()/(8400.0f))*(2*PI)*Rpulley;
+ 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
@@ -125,11 +107,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 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);
@@ -138,6 +120,7 @@
// horizontal motor
float currVelX = (currentX - prevX)/RATE;
prevX = currentX;
+ //pc.printf("currVe;X : %f \n\r", currVelX);
X_Controller.setProcessValue(currVelX);
//vertical motor
@@ -146,32 +129,45 @@
Y_Controller.setProcessValue(currVelY);
// compute the output
- float X_MotorOutput = X_Controller.compute();
- if (X_MotorOutput > 0)
+ 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
+ else if (X_MotorOutput <= -X_frictionTrheshold)
{
X_Direction.write(!CW); // left request
- X_Magnitude.write(-X_MotorOutput); //motor output will be negative and you need to write a pos value
+ 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 > 0)
+ if (Y_MotorOutput >= Y_frictionTrheshold)
{ // up request
Y_Direction.write(CW);
Y_Magnitude.write(Y_MotorOutput);
}
- else
+ else if (Y_MotorOutput <= -Y_frictionTrheshold)
{
Y_Direction.write(!CW); // down request
- Y_Magnitude.write(-Y_MotorOutput);
+ Y_Magnitude.write(fabs(Y_MotorOutput));
}
- i = 0;
+ else
+ {
+ Y_Magnitude.write(0);
+ Y_Direction.write(CW);
+ }
+ update_flag = false;
}
}
+ motorControl.detach();
}
void setUpdate_flag() {
@@ -179,56 +175,79 @@
}
void calibrate() {
- motorControl.detach();
+
// GO-TO pos (0,0)
- while ((X_Motor.getPulses() > margin) && (Y_Motor.getPulses() > margin)){
- if (X_Motor.getPulses() > margin){
+ 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(1.0); // WRONG, reqVel must be 1cm/s here
- } else { //motormagX 0
+ 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){
+ }
+ 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.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;
- 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)
+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)
{
- lMax = lCal;
- }
- float uCal = signal.readValue(3);
- if ( uCal > uMax)
- {
- uMax = uCal;
- }
- float dCal = signal.readValue(4);
- if ( dCal > dMax)
- {
- dMax = dCal;
- }
+ 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;
+ }
}
- return;
+ 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");
+
}