Project of Biorobotics
Dependencies: HIDScope MODSERIAL QEI mbed biquadFilter
Fork of TutorialPES by
main.cpp
- Committer:
- ThomBMT
- Date:
- 2018-10-30
- Revision:
- 9:355dd95199c3
- Parent:
- 8:e8734a254818
- Child:
- 10:05ad15c48388
File content as of revision 9:355dd95199c3:
#include "mbed.h" #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" MODSERIAL pc(USBTX, USBRX); DigitalOut DirectionPin1(D4); DigitalOut DirectionPin2(D7); PwmOut PwmPin1(D5); PwmOut PwmPin2(D6); 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_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); Ticker StateTicker; Ticker printTicker; HIDScope scope( 4 ); volatile float Bicep_Right = 0.0; volatile float Bicep_Left = 0.0; volatile float Tricep_Right = 0.0; volatile float Tricep_Left = 0.0; volatile const float pi = 3.1415926; 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 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; volatile float r_2; volatile const float r_3 = 0.035; volatile float w_1; volatile float w_2; volatile float Flex_Bi_R; 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; volatile float rad_m2; void Encoding() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); rad_m1 = rad_count * (float)counts1; 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(); Bicep_Left = emg1.read(); Tricep_Right = emg2.read(); Tricep_Left = emg3.read(); } void sample() { scope.set(0, emg0.read() ); scope.set(1, emg1.read() ); scope.set(2, emg2.read() ); scope.set(3, emg3.read() ); scope.send(); } void Inverse() { q_1= rad_m1; // uit Encoder q_2= rad_m2+(pi/2.0f); // uit Encoder r_1= -0.2f; r_2= -0.2f; float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2); 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\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 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\r\n", w_1); printf("%f\r\n", w_2); */ } /* void velocity1() { if (pot1.read()>0.5f) { // Clockwise rotation referenceVelocity1 = (pot1.read()-0.5f) * 2.0f; } else if (pot1.read() == 0.5f) { referenceVelocity1 = pot1.read() * 0.0f; } else if (pot1.read() < 0.5f) { // Counterclockwise rotation referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ; } } void velocity2() { if (pot2.read()>0.5f) { // Clockwise rotation referenceVelocity2 = (pot2.read()-0.5f) * 2.0f; } else if (pot2.read() == 0.5f) { referenceVelocity2 = pot2.read() * 0.0f; } else if (pot2.read() < 0.5f) { // Counterclockwise rotation referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ; } } */ void motor1() { float u_v1 = w_1; //referenceVelocity1 float u = u_v1/ (2.0f * pi); DirectionPin1 = u < 0.0f; PwmPin1 = fabs(u); } void motor2() { float u_v2 = w_2; //referenceVelocity2 float u = u_v2/ (2.0f * pi); 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; //static int ii; ii++; if (ii<=2500) { 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 && 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>5000 && ii<=6000) { if (ii == 5750) { pc.printf("Flex your left bicep now please. \r\n"); } //chillen } else if(ii>6000 && ii<8500) { m = m + emg1.read(); } 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>8500 && ii<=9500) { if (ii == 9250) { pc.printf("Flex your right tricep now please. \r\n"); } //chillen } else if (ii>9500 && ii<12000) { l = l + emg2.read(); } 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>12000 && ii <=13000) { if (ii == 12750) { pc.printf("Flex your left tricep now please. \r\n"); } //chillen } else if (ii>13000 && ii<15500) { k = k + emg3.read(); } 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; } } void OFF() { 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; 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() { switch (Active_State) { case Starting: OFF(); 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: //calibration actions //pc.printf("Calibration State"); Calibrating(); OFF(); if (!Knop1 && !Knop2) { 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(); Encoding(); break; case Homing: //Homing actions //pc.printf("Homing State"); Going_Home(); if (!Knop1 && !Knop2) { 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(); Encoding(); break; case Function: //pc.printf("Funtioning State"); if (Knop4==false) { 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(); motor1(); motor2(); break; default: pc.printf("UNKNOWN COMMAND"); } } int main() { pc.baud(115200); PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz StateTicker.attach(&StateMachine, 0.002); printTicker.attach(&Printing, 4.0); while(true) { } }