Thom Kuenen
Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of
TutorialPES
by 
Jurriën Bos
Diff: main.cpp
- Revision:
- 9:355dd95199c3
- Parent:
- 8:e8734a254818
- Child:
- 10:05ad15c48388
diff -r e8734a254818 -r 355dd95199c3 main.cpp
--- a/main.cpp Mon Oct 29 14:02:21 2018 +0000
+++ b/main.cpp Tue Oct 30 13:57:00 2018 +0000
@@ -11,13 +11,16 @@
DigitalIn Knop1(D2);
DigitalIn Knop2(D3);
DigitalIn Knop3(PTA4);
+DigitalIn Knop4(PTC6);
AnalogIn pot1 (A5);
AnalogIn pot2 (A4);
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );
-DigitalOut led(LED_GREEN);
+DigitalOut led_G(LED_GREEN);
+DigitalOut led_R(LED_RED);
+DigitalOut led_B(LED_BLUE);
QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
@@ -32,13 +35,17 @@
volatile float Tricep_Right = 0.0;
volatile float Tricep_Left = 0.0;
-volatile const float maxVelocity = 8.4; // in rad/s
volatile const float pi = 3.1415926;
-volatile const float rad_count = 0.0007479; // 2pi/8400;
+volatile const float rad_count = 0.0007479; // 2pi/8400;
+volatile const float maxVelocity = 2.0f * pi; // in rad/s
-volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
+volatile float referenceVelocity1 = 0.5; // dit is de gecentreerde waarde en dus de nulstand
volatile float referenceVelocity2 = 0.5;
+volatile int i = 0; // Led blink status
+volatile int ii = 0; // Calibratie timer
+volatile int iii = 0; // Start up timer
+
volatile float q_1;
volatile float q_2;
volatile float r_1;
@@ -51,12 +58,17 @@
volatile float Flex_Bi_L;
volatile float Flex_Tri_R;
volatile float Flex_Tri_L;
-
+volatile float Threshold_Bi_R;
+volatile float Threshold_Bi_L;
+volatile float Threshold_Tri_R;
+volatile float Threshold_Tri_L;
enum states{Starting, Calibration, Homing, Function};
volatile states Active_State = Starting;
+volatile float vx;
+volatile float vy;
volatile int counts1 ;
volatile int counts2 ;
volatile float rad_m1;
@@ -72,6 +84,53 @@
rad_m2 = rad_count * (float)counts2;
}
+void BlinkLed()
+{
+ if(i==1)
+ {
+ static int rr = 0;
+ rr++;
+ if (rr == 1)
+ {
+ led_R = !led_R;
+ }
+ else if (rr == 100)
+ {
+ rr = 0;
+ }
+ }
+ else if(i==2)
+ {
+ static int gg = 0;
+ gg++;
+ if (gg == 1)
+ {
+ led_G = !led_G;
+ }
+ else if (gg == 100)
+ {
+ gg = 0;
+ }
+ }
+ else if (i==3)
+ {
+ static int bb = 0;
+ bb++;
+ if (bb == 1)
+ {
+ led_B = !led_B;
+ }
+ else if (bb == 100)
+ {
+ bb = 0;
+ }
+ }
+ else
+ {
+ led_R=led_G=led_B=1;
+ }
+}
+
void EMG_Read()
{
Bicep_Right = emg0.read();
@@ -93,8 +152,8 @@
void Inverse()
{
- q_1= rad_m1; // uit Encoder
- q_2= rad_m2+(pi/2.0f); // uit Encoder
+ q_1= rad_m1; // uit Encoder
+ q_2= rad_m2+(pi/2.0f); // uit Encoder
r_1= -0.2f;
r_2= -0.2f;
@@ -102,26 +161,26 @@
float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
- float D = 1.0f/(u*z-x*y); // Determinant
- printf("Determinant is %f\n", D);
+ float D = 1.0f/(u*z-x*y); // Determinant
+ printf("Determinant is %f\r\n", D);
- float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
- float b = -D*x; // Inverse jacobian
- float c = -D*y; // Inverse jacobian
- float d = D*u; // Inverse jacobian
+ float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
+ float b = -D*x; // Inverse jacobian
+ float c = -D*y; // Inverse jacobian
+ float d = D*u; // Inverse jacobian
- float vx = 0.01f; // uit emg data
- float vy = 0.0f; // uit emg data
+ float vx = pot1.read();//0.01f; // uit emg data
+ float vy = pot2.read();//0.0f; // uit emg data
w_1 = vx*a+vy*b;
w_2 = vx*c+vy*d;
/*
- printf("%f\n", w_1);
- printf("%f\n", w_2);
+ printf("%f\r\n", w_1);
+ printf("%f\r\n", w_2);
*/
}
-
+/*
void velocity1()
{
if (pot1.read()>0.5f)
@@ -161,7 +220,7 @@
referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
}
}
-
+*/
void motor1()
{
float u_v1 = w_1; //referenceVelocity1
@@ -177,60 +236,128 @@
DirectionPin2 = u > 0.0f;
PwmPin2 = fabs(u);
}
-
+
void Calibrating()
{
static float n = 0.0;
static float m = 0.0;
static float l = 0.0;
static float k = 0.0;
-
- for(int ii=0; ii<=20000; ii++)
- {
- if (ii <2500)
+
+ //static int ii;
+ ii++;
+
+ if (ii<=2500)
{
- n = n + emg0.read();
+ if (ii == 0)
+ {
+ pc.printf("Relax your muscles please. \r\n");
+ }
+ else if (ii == 2250)
+ {
+ pc.printf("Flex your right bicep now please.\r\n");
+ }
+ //chillen
}
- else if (ii == 2500)
+ else if (ii>2500 && ii<5000) //
+ {
+ n = n + emg0.read();// dit wordt de variable naam na het filter
+ }
+ else if (ii == 5000)
{
Flex_Bi_R = n / 2500.0f;
+ pc.printf("You can relax your right bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_R);
}
- else if (ii>2500 && ii<=3500)
+ else if (ii>5000 && ii<=6000)
{
+ if (ii == 5750)
+ {
+ pc.printf("Flex your left bicep now please. \r\n");
+ }
//chillen
}
- else if(ii>3500 && ii<6000)
+ else if(ii>6000 && ii<8500)
{
m = m + emg1.read();
}
- else if (ii == 6000)
+ else if (ii == 8500)
{
Flex_Bi_L = m / 2500.0f;
+ pc.printf("You can relax your left bicep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Bi_L);
}
- else if (ii>6000 && ii<=7000)
+ else if (ii>8500 && ii<=9500)
{
+ if (ii == 9250)
+ {
+ pc.printf("Flex your right tricep now please. \r\n");
+ }
//chillen
}
- else if (ii>7000 && ii<9500)
+ else if (ii>9500 && ii<12000)
{
l = l + emg2.read();
}
- else if (ii == 9500)
+ else if (ii == 12000)
{
Flex_Tri_R = l / 2500.0f;
+ pc.printf("You can relax your right tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\n", Flex_Tri_R);
}
- else if (ii>9500 && ii <=10500)
+ else if (ii>12000 && ii <=13000)
{
+ if (ii == 12750)
+ {
+ pc.printf("Flex your left tricep now please. \r\n");
+ }
//chillen
}
- else if (ii>10500 && ii<13000)
+ else if (ii>13000 && ii<15500)
{
k = k + emg3.read();
}
- else if (ii == 13000)
+ else if (ii == 15500)
{
Flex_Tri_L = k / 2500.0f;
- }
+ pc.printf("You can relax your left tricep, thank you. \r\nYour mean flexing value was %f\r\n\r\nThe calibration has been completed, the system is now operatable. \r\n",Flex_Tri_L);
+ }
+
+Threshold_Bi_R = 0.75f * Flex_Bi_R;
+Threshold_Bi_L = 0.75f * Flex_Bi_L;
+Threshold_Tri_R = 0.75f * Flex_Tri_R;
+Threshold_Tri_L = 0.75f * Flex_Tri_L;
+
+ if (ii == 16500)
+ {
+ pc.printf("\r\nThreshold value right bicep = %f\r\nThreshold value left bicep = %f\r\nThreshold value right tricep = %f\r\nThreshold value left tricep = %f\r\n\r\n",Threshold_Bi_R,Threshold_Bi_L,Threshold_Tri_R,Threshold_Tri_L);
+ }
+ else if (ii == 20000)
+ {
+ pc.printf("\r\nAutomatic switch to Homing State\r\n");
+ Active_State = Homing;
+ }
+}
+
+void Start_Up()
+{
+ i++;
+ iii++;
+ if (iii == 1)
+ {
+ pc.printf("System is starting...\r\nWaiting for further input...\r\n");
+ }
+
+ else if (iii == 30000)
+ {
+ pc.printf("1 minute without input..\r\nReseting start-up...\r\n");
+ iii = 0;
+ }
+ else if (iii == 40001) // sleeping state is only added for designing purposes and will most likely never be used
+ { // when working with patients. Furthermore it cannot be reached automaticly
+ pc.printf("Sleeping... Press button 4 to wake me up!\r\n\r\n");
+ iii++;
+ }
+ else if (iii == 45000)
+ {
+ iii = 40000;
}
}
@@ -239,12 +366,58 @@
PwmPin1 = PwmPin2 = 0;
}
+void Going_Home()
+{
+ //Here we will reset the position of the arm back to the home state
+
+ if (counts1 == 0) // this 0 is a filler value and can later be determined to the angle of the
+ { // 0-state of the arm
+ PwmPin1=0.0f;
+ }
+ else if (counts1 > 0)
+ {
+ DirectionPin1 = true; //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden..
+ PwmPin1 = 0.6f;
+ }
+ else if (counts1 < 0)
+ {
+ DirectionPin1 = false;
+ PwmPin1 = 0.6f;
+ }
+
+ if (counts2 == 0) //Homing for M1 naar begin staat
+ {
+ PwmPin2=0.0f;
+ }
+ else if (counts2 > 0)
+ {
+ DirectionPin2 = true; //uitzoeken of dit klopt, is afhankelijk welke richting opgedraaid moet worden..
+ PwmPin2 = 0.6f;
+ }
+ else if (counts2 < 0)
+ {
+ DirectionPin2 = false;
+ PwmPin2 = 0.6f;
+ }
+
+ if (counts1 == 0 && counts2 == 0)
+ {
+ pc.printf("Homing is completed\r\nAutomatic switch to the Functioning State\r\n");
+ Active_State = Function;
+ }
+}
+
void Printing()
{
float v1 = PwmPin1 * maxVelocity;
float v2 = PwmPin2 * maxVelocity;
-
- pc.printf("q1 = %f [rad] \n q2 = %f [rad] \n q1dot = %f [rad/s] \n q2dot = %f [rad/s] \n\n\n\n\n", rad_m1, rad_m2, v1, v2);
+
+ if (Active_State == Function)
+ {
+ pc.printf("q1 = %f [rad] \r\nq2 = %f [rad] \r\nq1dot = %f [rad/s] \r\nq2dot = %f [rad/s] \r\n\r\n\r\n\r\n\r\n", rad_m1, rad_m2, v1, v2);
+ }
+
+ pc.printf("%f %f",vx, vy);
}
void StateMachine()
@@ -253,21 +426,20 @@
{
case Starting:
OFF();
- for (int i = 0; i<=20; i++)
- {
- led = !led;
- wait(0.05);
-
- if (i == 0)
- {
- pc.printf("Starting up..\n");
- }
- else if (i == 20)
- {
- Active_State = Calibration;
- pc.printf("Entering Calibration state \n");
- }
- }
+ Start_Up();
+ BlinkLed();
+
+ if (!Knop4 == true)
+ {
+ Active_State = Calibration;
+ pc.printf("Entering Calibration State \r\n");
+ }
+ else if (!Knop3 == true)
+ {
+ Active_State = Homing;
+ pc.printf("Entering Homing State \r\n");
+ }
+
break;
case Calibration:
@@ -276,11 +448,20 @@
Calibrating();
OFF();
- if (Knop1==false)
+
+ if (!Knop1 && !Knop2)
{
- pc.printf("Entering Homing state \n");
+ pc.printf("Switched to Sleeping State\r\n");
+ Active_State = Starting;
+ iii = 40001;
+ }
+ else if (Knop1==false)
+ {
+ pc.printf("Manual switch to Homing state \r\n");
Active_State = Homing;
}
+
+
sample();
EMG_Read();
@@ -290,11 +471,25 @@
case Homing:
//Homing actions
//pc.printf("Homing State");
- if (Knop2==false)
+ Going_Home();
+
+ if (!Knop1 && !Knop2)
{
- pc.printf("Entering Funtioning State \n");
+ pc.printf("Switched to Sleeping State\r\n");
+ Active_State = Starting;
+ iii = 40000;
+ }
+ else if (Knop2==false)
+ {
+ pc.printf("Manual switch to Funtioning State \r\n");
Active_State = Function;
}
+ else if (Knop3==false)
+ {
+ Active_State = Calibration;
+ pc.printf("Re-entering Calibration State \r\n");
+ }
+
sample();
EMG_Read();
@@ -304,17 +499,29 @@
case Function:
//pc.printf("Funtioning State");
- if (Knop3==false)
+ if (Knop4==false)
{
- pc.printf("Re-entering Calibration State \n");
+ pc.printf("Re-entering Calibration State \r\n");
Active_State = Calibration;
+ ii=0;
+ }
+ else if (Knop3==false)
+ {
+ pc.printf("Re-entering Homing State \r\n");
+ Active_State = Homing;
+ }
+ else if (!Knop1 && !Knop2)
+ {
+ pc.printf("Switched to Sleeping State\r\n");
+ Active_State = Starting;
+ iii = 40000;
}
sample();
EMG_Read();
Encoding();
- velocity1();
- velocity2();
+ //velocity1();
+ //velocity2();
motor1();
motor2();
break;