alles ingevoegd
Dependencies: FastPWM HIDScope MODSERIAL QEI mbed biquadFilter
Fork of deelPID by
Diff: main.cpp
- Revision:
- 5:8e326d07f125
- Parent:
- 4:93c4e826d11d
- Child:
- 6:405ec2bba6d0
--- a/main.cpp Wed Oct 31 12:05:10 2018 +0000 +++ b/main.cpp Wed Oct 31 16:08:45 2018 +0000 @@ -5,11 +5,13 @@ #include "HIDScope.h" #include "BiQuad.h" -InterruptIn buttoncal(D2); +DigitalIn ButtonCal(D2); //Button 1 DigitalOut gpo(D0); -DigitalOut led(LED_RED); +DigitalOut ledred(LED_RED); +DigitalOut ledblue(LED_BLUE); +DigitalOut ledgreen(LED_GREEN); AnalogIn pot1(A4); //POORTEN VERANDEREN -AnalogIn pot2(A3); //Beneden is I control op 0 gezet. //POORTEN veranderen +//AnalogIn pot2(A3); //Beneden is I control op 0 gezet. //POORTEN veranderen AnalogIn pot3(A5); //POORTEN VERANDEREN QEI encoder1(D10, D11, NC, 32); QEI encoder2(D12, D13, NC, 32); @@ -21,13 +23,13 @@ AnalogIn a0(A0); AnalogIn a1(A1); AnalogIn a2(A2); -//AnalogIn a3(A3); +AnalogIn a3(A3); MODSERIAL pc(USBTX, USBRX); //HIDSCOPE //Oppassen waar we HIDSCOPE aanroepen want nu voor meerdere dingen -//HIDScope scope(4); -//Ticker scopeTimer; +HIDScope scope(4); +Ticker scopeTimer; Ticker EMGTicker; @@ -100,12 +102,20 @@ int countstep = 0; //Demo variabelen -double vxMax = 2.0; -double vyMax = 2.0; +double vxMax = 4.0; +double vyMax = 4.0; int DemoStage = 0; +//States +enum states {WaitModusState, EMGCalibrationState, NormalOperatingModusState, DemoModusState}; +states State = WaitModusState; +//Calibration Time + +int countcalibration = 0; +double CalibrationTime = 20.0; //Tijd om te calibreren seconden + //EMGDINGEN void ReadEMG() @@ -113,7 +123,7 @@ EMGdata1 = a0.read(); // store values in the scope EMGdata2 = a1.read(); EMGdata3 = a2.read(); - EMGdata4 = 0.0; //a3.read(); + EMGdata4 = a3.read(); } // EMG High pass filters @@ -230,18 +240,27 @@ unit_vX = filt2kin (EMG_filt1, EMG_filt2, EMG_max1, EMG_max2); unit_vY = filt2kin (EMG_filt3, EMG_filt4, EMG_max3, EMG_max4); + if (fabs(unit_vX)<0.1) + { + unit_vX = 0.0; + } + if (fabs(unit_vY)<0.1) + { + unit_vY = 0.0; + } + - //scope.set(0, unit_Vx); - //scope.set(1, unit_Vy); - //scope.set(2, EMG_filt3); - //scope.set(3, EMG_filt4); + scope.set(0, unit_vX); + scope.set(1, unit_vY); + scope.set(2, EMG_filt1); + scope.set(3, EMG_filt2); count++; if (count == 100) { - pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4); + //pc.printf("EMG filt 1 = %lf \t EMG filt 2 = %lf \t EMG filt 3 = %lf \t EMG filt 4 = %lf \n\r", EMG_filt1, EMG_filt2, EMG_filt3, EMG_filt4); count = 0; } } @@ -395,13 +414,6 @@ } -void HomingSystem() //Manually place the arm in position q1 = 90degrees, q2 = 0 degrees -{ - q1Pos = 3.1416 / 2.0; - q2Pos = 0.0; - q1Ref = 3.1416 / 2.0; - q2Ref = 0.0; -} void DemonstrationPath() { @@ -455,14 +467,73 @@ +void CalibrationButton() +{ + ledred = 1; + ledgreen = 1; + ledblue = 0; + + EMG_max1 = 0.0001; + EMG_max2 = 0.0001; + EMG_max3 = 0.0001; + EMG_max4 = 0.0001; + + State = EMGCalibrationState; + + +} + +void EMGCalibration() +{ + if (0.95*EMG_filt1>EMG_max1) + { + EMG_max1 = 0.95*EMG_filt1; + } + if (0.95*EMG_filt2>EMG_max2) + + { + EMG_max2 = 0.95*EMG_filt2; + } + + if (0.95*EMG_filt3>EMG_max3) + { + EMG_max3 = 0.95*EMG_filt3; + } + if (0.95*EMG_filt4>EMG_max4) + { + EMG_max4 = 0.95*EMG_filt4; + } + +} + + + void CalibrationModus() { - EMG(); - //EMGCalibration(); + + EMG(); + EMGCalibration(); + + countcalibration++; + pc.printf("countcal = %i", countcalibration); + if (countcalibration >= (int)(CalibrationTime*1.0/TickerPeriod)) + { + State = NormalOperatingModusState; + countcalibration = 0; + } + } void DemoModus() // The ticker should call this function (in the switch statement) { + + //GainP1 = pot3.read() * 30; + //GainI1 = pot2.read() * 10; + //GainD1 = pot1.read() ; + //GainP2 = pot3.read() * 10; + //GainI2 = pot2.read() * 10; + //GainD2 = pot1.read() ; + DemonstrationPath(); inverse(); ReadCurrentPosition(); @@ -470,10 +541,15 @@ PIDControl(); MotorControl(); + //scope.set(0, q1Pos); + // scope.set(1, q1Ref); + //scope.set(2, q2Pos); + //scope.set(3, q2Ref); + count++; if (count ==400) { - pc.printf("q1Pos = %lf, q2Pos = %lf, q1Ref = %lf, q2Ref = %lf, xRef = %lf, yRef = %lf \n\r", 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", GainP1, GainI1, GainD1, q1Pos, q2Pos, q1Ref, q2Ref, xRef, yRef); count = 0; } } @@ -481,6 +557,10 @@ void NormalOperatingModus() { + ledred = 1; + ledgreen = 0; + ledblue = 1; + EMG(); InverseKinematics(); ReadCurrentPosition(); @@ -499,7 +579,7 @@ counter++; if (counter == 400) // print 1x per seconde waardes. { - pc.printf("GainP1 = %1f, GainI1 = %1f, GainD1 = %1f, q1Pos = %lf, q1Ref = %1f \n\r", GainP1, GainI1, GainD1, q1Pos, q1Ref); + pc.printf("xRef = %lf, q1Pos = %lf, q1Ref = %1f \n\r", xRef, q1Pos, q1Ref); counter = 0; } if (countstep == 4000) @@ -511,38 +591,57 @@ } -//enum states {WaitModus, EMGCalibration, NormalOperatingModus, DemonstrationModus}; + -/*void StateMachine() +void StateMachine() { + + + if (ButtonCal.read() == 0) + { + CalibrationButton(); + pc.printf("print iets"); + } + switch(State) { - case WaitModus: //aanmaken + case WaitModusState: //aanmaken EMG(); + pc.printf("Wait \n\r"); break; - case EMGCalibration: - EMGCalibration(); + case EMGCalibrationState: + CalibrationModus(); + //pc.printf("EMG CAL \n\r"); break; - case NormalOperatingModus: + case NormalOperatingModusState: NormalOperatingModus(); + //pc.printf("NOMS \n\r"); break; - case DemoModus: - DemoModus(); //Goede naam geven.. + case DemoModusState: + DemoModus(); + pc.printf("Demo \n\r"); break; default : } } -*/ + int main() { pc.baud(115200); - pc.printf("nog 10 seconden \n\r"); + /* + GainP1 = pot3.read() * 10; + GainI1 = pot2.read() * 10; + GainD1 = pot1.read(); + */ + /* + 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"); wait(3.0); + */ //BiQuad chains bqc1.add( &HP_emg1 ).add( &Notch_emg1 ); @@ -560,14 +659,17 @@ Motor1PWM.period(1.0/frequency_pwm); // T = 1/f Motor2PWM.period(1.0/frequency_pwm); // T = 1/f - HomingSystem(); + //Emg Calibratie button + //ButtonCal.fall(&CalibrationButton); + - Kikker.attach(DemoModus, TickerPeriod); - //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); + + Kikker.attach(StateMachine, TickerPeriod); + // scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); //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); //.