Alle subfiles samengevoegd
Dependencies: HIDScope biquadFilter mbed MODSERIAL FastPWM QEI
main.cpp
- Committer:
- s1600907
- Date:
- 2016-10-20
- Revision:
- 0:c96cf9760185
- Child:
- 1:e1267e72ade8
File content as of revision 0:c96cf9760185:
#include "mbed.h" #include "HIDScope.h" #include "BiQuad.h" #include "math.h" #include "iostream" #define SERIAL_BAUD 115200 Serial pc(USBTX,USBRX); //EMG aansluitingen AnalogIn emg0( A0 ); //Biceps Rechts AnalogIn emg1( A1 ); //Bicpes Links AnalogIn emg2( A2 ); //Upper leg //Leds DigitalOut led(LED1); // lampje om te zien dat het programma werkt DigitalOut led2(LED2); DigitalOut led3(LED3); //HIDScope HIDScope scope( 6 ); // aantal channels op de HIDscope //Button om Calibratie te beëindigen InterruptIn Button1(PTC6); // DIT WORDT EEN ANDERE BUTTON // Tickers Ticker sample_timer; Ticker TickerCalculationsForTheta; //BiQuad // making the biquads and chains; imported from matlab BiQuadChain bqc; //Chain for right biceps BiQuadChain bqc2; //Chain for left biceps BiQuadChain bqc3; //Chain for Upper leg // 1 to 3 are for right biceps BiQuad bq1( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 ); BiQuad bq2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); BiQuad bq3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); // 4 to 6 are for left biceps BiQuad bq4( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 ); BiQuad bq5( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); BiQuad bq6( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 ); // 7 to 9 are for upper leg BiQuad bq7( 9.14969e-01, -1.82994e+00, 9.14969e-01, -1.82269e+00, 8.37182e-01 ); BiQuad bq8( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 ); BiQuad bq9( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 ); // Variabelen voor filter // spier EMG niet boven treshhold volatile bool BicepsLeft = false; volatile bool BicepsRight = false; volatile bool Leg = false; // filtergo uit zetten, zodat deze niet fout gaat. volatile bool filtergo = false; // Variabelen voor MotorHoekBerekenen double x = 0.0; // beginpositie x en y double y = 305.5; volatile const double pi = 3.14159265359; double Theta1Gear = 1.0/3.0*pi; // Beginpositie op 60 graden double Theta2Gear = 1.0/3.0*pi; double Theta1 = Theta1Gear*42/12; // Beginpositie van MotorHoek double Theta2 = Theta2Gear*42/12; double Fr_Speed_Theta = 100.0; // frequentie in Hz waarmee theta wordt uigerekend bool Calibration = true; // beginnen met calibreren volatile bool Stepper_State = false; // false = we zijn niet bezig met flippen /////////////////////// functies voor filters ///////////////////////////////////////////// void sample() { /* Sample function samples the emg0 (of the rightBiceps) samples the emg1 (of the leftBiceps) samples the emg2 (of the Upper leg) filter the singals sends it to HIDScope */ // Rechts Biceps double inRechts = emg0.read(); // EMG signal double FilterRechts = bqc.step(inRechts); // High pass + Notch (50 HZ) double RectifyRechts = fabs(FilterRechts); // Rectify double outRechts = bq3.step(RectifyRechts); // Low pass // Links Biceps double inLinks = emg1.read(); // EMG signal double FilterLinks = bqc2.step(inLinks); // High pass + Notch (50 HZ) double RectifyLinks = fabs(FilterLinks); // Rectify double outLinks = bq6.step(RectifyLinks); // Low pass // Upper leg double inBeen = emg2.read(); // EMG signal double FilterBeen = bqc3.step(inBeen); // High pass + Notch (50 HZ) double RectifyBeen = fabs(FilterBeen); // Rectify double outBeen = bq9.step(RectifyBeen); // Low pass /* EMG signal in channel 0 (the first channel) and the filtered EMG signal in channel 1 (the second channel) of the HIDscope */ scope.set(0,inRechts); scope.set(1,outRechts); scope.set(2,inLinks); scope.set(3,outLinks); scope.set(4,inBeen); scope.set(5,outBeen); // send all channels to the PC at once scope.send(); // To indicate that the function is working, the LED is on if (outRechts >= 0.015){ led2 = 0; led = 1; BicepsRight = true; } else{ led2 = 1; led = 0; BicepsRight = false; } // If Biceps links is actuated then: if (outLinks >= 0.015){ led3 = 0; led = 1; BicepsLeft = true; } else{ led3 = 1; led = 0; BicepsLeft = false; } // If upper leg is actuated then: if (outBeen >= 0.01){ led3 = 0; led2 = 0; led = 1; Leg = true; } else{ led3 = 1; led2 = 1; led = 0; Leg = false; } filtergo = false; } // zet de filter aan void filter() { filtergo=true; } ///////////////////// functies voor motorhoekberekenen ////////////////////////// // vorige x berekenen double Calc_Prev_x () { double Prev_x = x; //pc.printf("prev x = %f\r\n", Prev_x); return Prev_x; } // vorige y berekenen double Calc_Prev_y () { double Prev_y = y; //pc.printf("prev y = %f\r\n", Prev_y); return Prev_y; } // berekenen van de nieuwe x en y waardes bool CalcXY (bool BicepsLeft, bool BicepsRight, bool Leg, bool Stepper_State) { double Step = 10/Fr_Speed_Theta ; //10 mm per seconde afleggen if (BicepsLeft==true && BicepsRight==true && Leg==true && Stepper_State==false) { Stepper_State = true; // we zijn aan het flipper dus geen andere dingen doen // flipper functie aanroepen } else if (BicepsLeft==true && BicepsRight==false && Leg==false && Stepper_State==false) { x = x - Step; } else if (BicepsLeft==false && BicepsRight==true && Leg==false && Stepper_State==false) { x = x + Step; // naar Right bewegen } else if (BicepsLeft==true && BicepsRight==true && Leg==false && Stepper_State==false) { y = y + Step; // naar voren bewegen } else if (BicepsLeft==false && BicepsRight==true && Leg==true && Stepper_State==false) { y = y - Step; // naar achter bewegen } else if (BicepsLeft==false && BicepsRight==false && Leg==false || Stepper_State==true) { } // Grenswaardes LET OP: ARMEN MISSCHIEN GEBLOKKEERD DOOR BALK AAN DE BINNENKANT if (x > 200) { x = 200; } else if (x < -200) { x = -200; } if (y > 306) { y = 306; } else if (y < 50) { y = 50; // GOKJE, UITPROBEREN } pc.printf("x = %f, y = %f\r\n", x, y); return Stepper_State; } // diagonaal berekenen voor linker arm double CalcDia1 (double x, double y) { double a = 50.0; // de afstand van gekozen assenstelsel tot de armas (assenstelsel precies in het midden) KEERTJE NAMETEN double BV1 = sqrt(pow((a+x),2) + pow(y,2)); // diagonaal (afstand van armas tot locatie) berekenen double Dia1 = pow(BV1,2)/(2*BV1); // berekenen van de afstand oorsprong tot diagonaal //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia1, x, y); return Dia1; } // diagonaal berekenen voor rechter arm double CalcDia2 (double x, double y) { double a = 50.0; double BV2 = sqrt(pow((x-a),2) + pow(y,2)); // zelfde nog een keer doen maar nu voor de rechter arm double Dia2 = pow(BV2,2)/(2*BV2); //pc.printf("dia = %f, x = %f, y= %f\r\n", Dia2, x, y); return Dia2; } // calculate Theta1 void CalcTheta1 (double Dia1, double Prev_x, double Prev_y) { double a = 50.0; double Bar = 200.0; // lengte van de armen double Prev_Theta1_Gear = Theta1Gear; double MaxThetaGear = pi - 30.0*pi/180.0; // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel) // Hoek berekenen van het grote tandwiel (gear) if (x > -a) { Theta1Gear = pi - atan(y/(x+a)) - acos(Dia1/Bar); } else if (x > -a) { Theta1Gear = pi - (pi + atan(y/(x+a))) - acos(Dia1/Bar); } else { // als x=-a Theta1Gear = 0.5*pi - acos(Dia1/Bar); } // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes) if (Theta1Gear >= MaxThetaGear) { // is de maximale hoek in radialen Theta1Gear = Prev_Theta1_Gear; x = Prev_x; y = Prev_y; } // omrekenen van grote tandwiel naar motortandwiel Theta1 = Theta1Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta1, Theta1Gear, Prev_Theta1_Gear); } void CalcTheta2 (double Dia2, double Prev_x, double Prev_y) { double a = 50.0; double Bar = 200.0; // lengte van de armen double Prev_Theta2_Gear = Theta2Gear; double MaxThetaGear = pi - 30.0*pi/180.0; // de hoek voordat arm het opstakel raakt (max hoek is 24.9 tussen arm en opstakel) // Hoek berekenen van het grote tandwiel (gear) if (x < a) { Theta2Gear = pi + atan(y/(x-a)) - acos(Dia2/Bar); } else if (x > a) { Theta2Gear = pi - (pi - atan(y/(x-a))) - acos(Dia2/Bar); } else { // als x=a Theta2Gear = 0.5*pi - acos(Dia2/Bar); } // limiten (als de hoek te groot wordt, reset dan de hoek en de x en y waardes) if (Theta2Gear >= MaxThetaGear) { Theta2Gear = Prev_Theta2_Gear; x = Prev_x; y = Prev_y; } // omrekenen van grote tandwiel naar motortandwiel Theta2 = Theta2Gear*42.0/12.0; // grote tandwiel heeft 42 tanden. Motortandwiel heeft er 12. pc.printf("thetaMotor = %f, thetaGear = %f, Prev_Gear = %f\r\n", Theta2, Theta2Gear, Prev_Theta2_Gear); } void CalculationsForTheta () { double Prev_x = Calc_Prev_x (); double Prev_y = Calc_Prev_y (); bool Stepper_State = CalcXY (BicepsLeft, BicepsRight, Leg, Stepper_State); double Dia1 = CalcDia1 (x, y); double Dia2 = CalcDia2 (x, y); CalcTheta1 (Dia1, Prev_x, Prev_y); CalcTheta2 (Dia2, Prev_x, Prev_y); } // als de button ingedrukt wordt dan stoppen we met calibreren void EndCalibration () { Calibration = false; } //////////////// functies voor aansturing motoren /////////////////////////////// ////////////////////////// main loop //////////////////////////////////////////// int main() { pc.baud(SERIAL_BAUD); pc.printf("\r\n Nieuwe code uitproberen :) \r\n"); /* making the biquad chain for filtering the emg notch and high pass */ bqc.add( &bq1 ).add( &bq2 ); bqc2.add( &bq4 ).add( &bq5 ); bqc3.add( &bq7 ).add( &bq8 ); // Calibreren while (Calibration == true) { // potmeter dingen aanpassen pc.printf("calibreren is bezig\r\n"); Button1.fall(&EndCalibration); } /**Attach the 'sample' function to the timer 'sample_timer'. * this ensures that 'sample' is executed every 0.002 seconds = 500 Hz */ sample_timer.attach(&filter, 0.002); // extra stap, zodat er geen op onthoudt komt. while (filtergo == true) { sample(); } // ticker voor hoek berekenen aanzetten TickerCalculationsForTheta.attach(&CalculationsForTheta,2); ///Fr_Speed_Theta //empty loop, sample() is executed periodically while(1) {} }