alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
Diff: main.cpp
- Revision:
- 8:fa268d163bbd
- Parent:
- 7:de221f894d3b
diff -r de221f894d3b -r fa268d163bbd main.cpp --- a/main.cpp Fri Nov 02 08:15:25 2018 +0000 +++ b/main.cpp Sat Nov 03 09:51:42 2018 +0000 @@ -1,18 +1,19 @@ #include "mbed.h" #include "FastPWM.h" -#include "MODSERIAL.h" +//#include "MODSERIAL.h" #include "QEI.h" -#include "HIDScope.h" +//#include "HIDScope.h" #include "BiQuad.h" DigitalIn ButtonCal(D2); //Button 1 +DigitalIn ButtonDemo(D1);; //Button 2 DigitalOut gpo(D0); DigitalOut ledred(LED_RED); DigitalOut ledblue(LED_BLUE); DigitalOut ledgreen(LED_GREEN); -AnalogIn pot1(A4); //POORTEN VERANDEREN +//AnalogIn pot1(A4); //POORTEN VERANDEREN //AnalogIn pot2(A3); //Beneden is I control op 0 gezet. //POORTEN veranderen -AnalogIn pot3(A5); //POORTEN VERANDEREN +//AnalogIn pot3(A5); //POORTEN VERANDEREN QEI encoder1(D10, D11, NC, 32); QEI encoder2(D12, D13, NC, 32); FastPWM Motor1PWM(D6); @@ -21,23 +22,24 @@ DigitalOut Motor2Direction(D4); //EMG -/* + AnalogIn a0(A0); AnalogIn a1(A1); AnalogIn a2(A2); AnalogIn a3(A3); -*/ + -AnalogIn Ppot(A0); +/*AnalogIn Ppot(A0); AnalogIn Ipot(A1); AnalogIn Dpot(A2); +*/ -MODSERIAL pc(USBTX, USBRX); +//MODSERIAL pc(USBTX, USBRX); //HIDSCOPE //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen -HIDScope scope(6); -Ticker scopeTimer; +//HIDScope scope(6); +//Ticker scopeTimer; Ticker EMGTicker; @@ -126,7 +128,7 @@ //States enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState}; -states State = DemoModusState; +states State = WaitModusState; //Calibration Time @@ -138,12 +140,11 @@ void ReadEMG() { -/* EMGdata1 = a0.read(); // store values in the scope EMGdata2 = a1.read(); EMGdata3 = a2.read(); EMGdata4 = a3.read(); -*/ + } // EMG High pass filters @@ -270,19 +271,19 @@ } - scope.set(0, EMG_filt1); +/* scope.set(0, EMG_filt1); scope.set(1, EMG_filt2); scope.set(2, unit_vX); scope.set(3, EMG_filt3); scope.set(4, EMG_filt4); scope.set(5, unit_vY); - + */ count++; if (count == 100) { - pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref); + //pc.printf("xRef = %1f, yRef = %1f, q2Ref = %1f, q1Ref = %1f \n\r", xRef, yRef, q2Ref, q1Ref); count = 0; } } @@ -573,7 +574,7 @@ EMGCalibration(); countcalibration++; - pc.printf("countcal = %i", countcalibration); + //pc.printf("countcal = %i", countcalibration); if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod)) { State = NormalOperatingModusState; @@ -585,15 +586,16 @@ void DemoModus() // The ticker should call this function (in the switch statement) { - GainP1 = 40.0;//Ppot.read()*100.0; + /*GainP1 = 40.0;//Ppot.read()*100.0; GainI1 = 17.0;//Ipot.read()*20.0; GainD1 = 2.0;//Dpot.read()*20.0; GainP2 = Ppot.read()*100.0; GainI2 = Ipot.read()*20.0; GainD2 = Dpot.read()*20.0; + */ - //DemonstrationPath(); - TestPath(); + DemonstrationPath(); + //TestPath(); inverse(); ReadCurrentPosition(); UpdateError(); @@ -608,7 +610,7 @@ count++; if (count ==400) { - pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", GainP2, GainI2, GainD2, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); + //pc.printf("GainP = %lf, GainI = %lf, GainD = %lf, q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", GainP2, GainI2, GainD2, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); count = 0; } } @@ -659,14 +661,24 @@ if (ButtonCal.read() == 0) { CalibrationButton(); - pc.printf("print iets"); + // pc.printf("print iets"); + Motor1PWM.write(0.0); + Motor2PWM.write(0.0); + } + if (ButtonDemo.read() == 0) + { + State = DemoModusState; + + ledred = 1; + ledgreen = 0; + ledblue = 0; } switch(State) { case WaitModusState: //aanmaken EMG(); - pc.printf("Wait \n\r"); + // pc.printf("Wait \n\r"); break; case EMGCalibrationState: CalibrationModus(); @@ -682,13 +694,14 @@ break; default : } + } int main() { - pc.baud(115200); + //pc.baud(115200); /* GainP1 = pot3.read() * 10; @@ -696,9 +709,9 @@ GainD1 = pot1.read(); */ - pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1); + //pc.printf("GainP = %lf, GainI = %lf, GainP = %lf, nog 10 seconden \n\r", GainP1, GainI1, GainD1); wait(7.0); - pc.printf("nog 3 seconden \n\r"); + //pc.printf("nog 3 seconden \n\r"); wait(3.0); @@ -715,8 +728,8 @@ } double frequency_pwm = 16700.0; //16.7 kHz PWM - Motor1PWM.period_us(1.0/frequency_pwm); // T = 1/f - Motor2PWM.period_us(1.0/frequency_pwm); // T = 1/f + Motor1PWM.period(1.0/frequency_pwm); // T = 1/f + Motor2PWM.period(1.0/frequency_pwm); // T = 1/f //Emg Calibratie button //ButtonCal.fall(&CalibrationButton); @@ -728,7 +741,7 @@ //Onderstaande stond in EMG deel, kijken hoe en wat met tickers!! // Attach the HIDScope::send method from the scope object to the timer at 50Hz - scopeTimer.attach_us(&scope, &HIDScope::send, 5e3); + //scopeTimer.attach_us(&scope, &HIDScope::send, 5e3); //EMGTicker.attach_us(EMG_filtering, 5e3); //.