Projectgroep 20 Biorobotics
State Machine, bezig met mooimaken
Dependencies: Encoder HIDScope MODSERIAL biquadFilter mbed
Fork of
vanEMGnaarMOTORPauline_States_nacht
by 
Projectgroep 20 Biorobotics
Diff: main.cpp
- Revision:
- 28:19cccdd68b5b
- Parent:
- 27:2d9f172c66ad
- Child:
- 29:69cc48b3feaa
--- a/main.cpp Mon Nov 06 14:57:15 2017 +0000
+++ b/main.cpp Mon Nov 06 15:52:46 2017 +0000
@@ -5,29 +5,43 @@
#include "encoder.h"
#include "MODSERIAL.h"
+// Variables
//State Machine and calibration
enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
States State;
-int Counter;
bool Position_controller_on;
-double looptime = 0.002f;
DigitalIn button (D1);
-//Encoder/motor
+//Buttons en leds voor calibration
+DigitalIn button1(PTA4);
+DigitalOut led(D2);
+
+//MVC for calibration
+double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
+//MEAN for calibration - rest
+double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
+double emgMEANSUBLB; double emgMEANSUBRB; double emgMEANSUBLT; double emgMEANSUBRT;
+double emgSUMLB; double emgSUMRB; double emgSUMLT; double emgSUMRT;
+
+bool caldone = false;
+int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
+
+int Timescalibration = 0;
+int TimescalibrationREST = 0;
+
+//Encoder and motor
double Huidigepositie1;
double Huidigepositie2;
double motorValue1;
double motorValue2;
-
-//global variables Motor
Ticker Treecko; //We make an awesome ticker for our control system
-Ticker printer;
PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
PwmOut M2E(D5);
DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
Encoder motor1(D13,D12,true);
Encoder motor2(D9,D8,true);
DigitalOut M2D(D4);
+double PwmPeriod = 1.0/5000.0;
//Demonstration
AnalogIn potMeter2(A1);
@@ -35,40 +49,14 @@
MODSERIAL pc(USBTX,USBRX);
-double PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
+//PID
const double Ts = 0.002f; // tickettijd/ sample time
double e_prev = 0;
double e_int = 0;
double e_prev2 = 0;
double e_int2 = 0;
-double tijdstap = 0.002;
-volatile double LBF;
-volatile double RBF;
-volatile double LTF;
-volatile double RTF;
-
-//buttons en leds voor calibration
-DigitalIn button1(PTA4);
-
-DigitalOut led(D2);
-
-//MVC for calibration
-double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
-//MEAN for calibration - rest
-double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
-
-double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
-double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;
-
-
-bool caldone = false;
-int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
-
-int Timescalibration = 0;
-int TimescalibrationREST = 0;
-
-// Filters
+// EMG and Filters
// Biquad filters voor Left Biceps (LB): Notch, High-pass and Low-pass filter
BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFLB;
@@ -129,28 +117,30 @@
double emgAbsHPRT;
double emgLPRT;
-double f = 500; // frequency
-double dt = 1/f; // sample frequency
-
-AnalogIn emgLB(A0); // EMG lezen
+AnalogIn emgLB(A0); // read EMG
AnalogIn emgRB(A1);
AnalogIn emgLT(A2);
AnalogIn emgRT(A3);
-// variables RKI
+volatile double LBF;
+volatile double RBF;
+volatile double LTF;
+volatile double RTF;
+
+// RKI
double pi = 3.14159265359;
-double q1 = (pi/2); //Reference position hoek 1 in radiance
-double q2 = -(pi/2); //Reference position hoek 2 in radiance
-const double L1 = 0.30; //Length arm 1 in mm
-const double L2 = 0.38; //Length arm 2 in mm
-double B1 = 1; //Friction constant motor 1
-double B2 = 1; //Friction constant motor 2
-double K = 1; //Spring constant movement from end-effector position to setpoint position
-double Tijd = 1; //Timestep value
-double Rsx = 0.38; //Reference x-component of the setpoint radius
-double Rsy = 0.30; //Reference y-component of the setpoint radius
-double refP = 0; //Reference position motor 1
-double refP2 = 0; //Reference position motor 2
+double q1 = (pi/2); //Reference position angle 1 in radiance
+double q2 = -(pi/2); //Reference position angle 2 in radiance
+const double L1 = 0.30; //Length arm 1 in mm
+const double L2 = 0.38; //Length arm 2 in mm
+double B1 = 1; //Friction constant motor 1
+double B2 = 1; //Friction constant motor 2
+double K = 1; //Spring constant movement from end-effector position to setpoint position
+double Tijd = 1; //Timestep value
+double Rsx = 0.38; //Reference x-component of the setpoint radius
+double Rsy = 0.30; //Reference y-component of the setpoint radius
+double refP = 0; //Reference position motor 1
+double refP2 = 0; //Reference position motor 2
double Rex = cos(q1)*L1 - sin(q2)*L2; //The x-component of the end-effector radius
double Rey = sin(q1)*L1 + cos(q2)*L2; //The y-component of the end-effector radius
double R1x = 0; //The x-component of the joint 1 radius
@@ -164,41 +154,36 @@
double w1= 0;
double w2= 0;
-
+// Functions
void Filteren()
{
- emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter
+ emgNotchLB = NFLB.step(emgLB.read() ); // Notch filter
emgHPLB = HPFLB.step(emgNotchLB); // High-pass filter: also normalises around 0.
- emgAbsHPLB = abs(emgHPLB); // Take absolute value
- emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope
- emgMEANSUBLB = emgLPLB - RESTMEANLB; //substract the restmean value
- LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
+ emgAbsHPLB = abs(emgHPLB); // Take absolute value
+ emgLPLB = LPFLB.step(emgAbsHPLB); // Low-pass filter: creates envelope
+ emgMEANSUBLB = emgLPLB - RESTMEANLB; // Substract the restmean value
+ LBF = emgLPLB/MVCLB; // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
- emgNotchRB = NFRB.step(emgRB.read()); // Notch filter
- emgHPRB = HPFRB.step(emgNotchRB); // High-pass filter: also normalises around 0.
- emgAbsHPRB = abs(emgHPRB); // Take absolute value
- emgLPRB = LPFRB.step(emgAbsHPRB); // Low-pass filter: creates envelope
+ emgNotchRB = NFRB.step(emgRB.read());
+ emgHPRB = HPFRB.step(emgNotchRB);
+ emgAbsHPRB = abs(emgHPRB);
+ emgLPRB = LPFRB.step(emgAbsHPRB);
emgMEANSUBLB = emgLPLB - RESTMEANLB;
- RBF = emgLPRB/MVCRB; // Scale to maximum signal: useful for motor
+ RBF = emgLPRB/MVCRB;
- emgNotchLT = NFLT.step(emgLT.read() ); // Notch filter
- emgHPLT = HPFLT.step(emgNotchLT); // High-pass filter: also normalises around 0.
- emgAbsHPLT = abs(emgHPLT); // Take absolute value
- emgLPLT = LPFLT.step(emgAbsHPLT); // Low-pass filter: creates envelope
- emgMEANSUBLT = emgLPLT - RESTMEANLT; //substract the restmean value
- LTF = emgLPLT/MVCLT; // Scale to maximum signal: useful for motor
+ emgNotchLT = NFLT.step(emgLT.read() );
+ emgHPLT = HPFLT.step(emgNotchLT);
+ emgAbsHPLT = abs(emgHPLT);
+ emgLPLT = LPFLT.step(emgAbsHPLT);
+ emgMEANSUBLT = emgLPLT - RESTMEANLT;
+ LTF = emgLPLT/MVCLT;
- emgNotchRT = NFRT.step(emgRT.read() ); // Notch filter
- emgHPRT = HPFRT.step(emgNotchRT); // High-pass filter: also normalises around 0.
- emgAbsHPRT = abs(emgHPRT); // Take absolute value
- emgLPRT = LPFRT.step(emgAbsHPRT); // Low-pass filter: creates envelope
- emgMEANSUBRT = emgLPRT - RESTMEANRT; //substract the restmean value
- RTF = emgLPRT/MVCRT; // Scale to maximum signal: useful for motor
-
- //if (emgFiltered >1)
- //{
- // emgFiltered=1.00;
- //}
+ emgNotchRT = NFRT.step(emgRT.read() );
+ emgHPRT = HPFRT.step(emgNotchRT);
+ emgAbsHPRT = abs(emgHPRT);
+ emgLPRT = LPFRT.step(emgAbsHPRT);
+ emgMEANSUBRT = emgLPRT - RESTMEANRT;
+ RTF = emgLPRT/MVCRT;
}
void CalibrationEMG()
@@ -245,7 +230,7 @@
}
if(Timescalibration>2000 && Timescalibration<3000)
{
- pc.printf("maximum linker biceps \r\n");
+ pc.printf("maximum left biceps \r\n");
led = 1;
emgNotchLB = NFLB.step(emgLB.read() );
emgHPLB = HPFLB.step(emgNotchLB);
@@ -260,7 +245,7 @@
if(Timescalibration>3000 && Timescalibration<4000)
{
- pc.printf(" maximum rechter biceps \r\n");
+ pc.printf(" maximum right biceps \r\n");
led = 0;
emgNotchRB = NFRB.step(emgRB.read());
emgHPRB = HPFRB.step(emgNotchRB);
@@ -275,7 +260,7 @@
if(Timescalibration>4000 && Timescalibration<5000)
{
- pc.printf("maximum linker triceps \r\n");
+ pc.printf("maximum left triceps \r\n");
led = 1;
emgNotchLT = NFLT.step(emgLT.read() );
emgHPLT = HPFLT.step(emgNotchLT);
@@ -290,7 +275,7 @@
if(Timescalibration>5000 && Timescalibration<6000)
{
- pc.printf("maximum rechter triceps");
+ pc.printf("maximum right triceps");
emgNotchRT = NFRT.step(emgRT.read() );
emgHPRT = HPFRT.step(emgNotchRT);
emgAbsHPRT = abs(emgHPRT);
@@ -304,10 +289,9 @@
if(Timescalibration>6000)
{
- pc.printf("calibratie afgelopen");
+ pc.printf("calibration finished");
State = SelectDevice;
}
-
}
void RKI()
@@ -327,7 +311,7 @@
int maxwaarde = 4096; // = 64x64
refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
- refP2 = (( q1 + q2)/(2*pi))*maxwaarde; //Get reference positions was eerst 0.5*pi
+ refP2 = (( q1 + q2)/(2*pi))*maxwaarde; //Get reference positions
}
void SetpointRobot()
@@ -336,39 +320,23 @@
double Potmeterwaarde1 = potMeter1.read();
if (Potmeterwaarde2>0.6) {
- Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+ Rsx += 0.001; //increases 1 mm when potmetervalue above 0.6
}
else if (Potmeterwaarde2<0.4) {
- Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
+ Rsx -= 0.001; //decreases 1 mm when potmetervalue below 0.4
}
- else { //de x-waarde van de setpoint verandert niet
+ else { //x value of setpoint doesn't change
}
- if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
+ if (Potmeterwaarde1>0.6) { //increases 1 mm when potmetervalue above 0.6
Rsy += 0.001;
}
- else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
+ else if (Potmeterwaarde1<0.4) { //decreases 1 mm when potmetervalue below 0.4
Rsy -= 0.001;
}
- else { //de y-waarde van de setpoint verandert niet
+ else { //y value of setpoint doesn't change
}
}
-
-double GetReferencePosition()
-{
- double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
- int maxwaarde = 4096; // = 64x64
- double refP = Potmeterwaarde*maxwaarde;
- return refP; // value between 0 and 4096
-}
-
-double GetReferencePosition2()
-{
- double potmeterwaarde2 = potMeter1.read();
- int maxwaarde2 = 4096; // = 64x64
- double refP2 = potmeterwaarde2*maxwaarde2;
- return refP2; // value between 0 and 4096
-}
double FeedBackControl(double error, double &e_prev, double &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
@@ -415,10 +383,10 @@
M1D = 1;
}
if (fabs(motorValue) > 1) {
- M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
+ M1E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
}
else {
- M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
+ M1E = fabs(motorValue); //absolute velocity determined, motor is "off" at value of 0
}
}
@@ -431,26 +399,22 @@
M2D = 0;
}
if (fabs(motorValue2) > 1) {
- M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
+ M2E = 1; //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
}
else {
- M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
+ M2E = fabs(motorValue2); //absolute velocity determined, motor is "off" at value of 0
}
}
void MeasureAndControl(void)
{
- //SetpointRobot();
- // RKI aanroepen
RKI();
// control of 1st motor
- //double refP = GetReferencePosition(); //KOMT UIT RKI
double Huidigepositie = motor1.getPosition();
double error = (refP - Huidigepositie);// make an error
double motorValue = FeedBackControl(error, e_prev, e_int);
SetMotor1(motorValue);
// control of 2nd motor
- //double refP2 = GetReferencePosition2();
double Huidigepositie2 = motor2.getPosition();
double error2 = (refP2 - Huidigepositie2);// make an error
double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
@@ -460,23 +424,22 @@
void changePosition () // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
{
- if (RBF>0.6) {
+ if (RBF>0.5) {
Rsx +=0.001; // hoe veel verder gaat hij? 1 cm? 10 cm?
}
else {}
- if (RTF>0.6) {
+ if (RTF>0.5) {
Rsx -=0.001;
}
else {}
- if (LBF>0.6) {
+ if (LBF>0.5) {
Rsy +=0.001;
}
else {}
- if (LTF>0.6) {
+ if (LTF>0.5) {
Rsy -=0.001;
}
else {}
- pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
}
void Loop_funtion()
@@ -502,7 +465,9 @@
MeasureAndControl();
break;
case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
- case Demonstration: // COntrol with potmeters
+ case Demonstration: // Control with potmeters
+ SetpointRobot();
+ MeasureAndControl();
break;
}
}
@@ -537,7 +502,7 @@
//motor
M1E.period(PwmPeriod); //set PWMposition at 5000hz
//Ticker
- //Treecko.attach(MeasureAndControl, tijdstap); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
+ //Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
//functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
// printer.attach(Tickerfunctie,0.4);