![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 3:6e28b992b99e
- Parent:
- 2:7ea5ae2287a7
- Child:
- 4:1e8da6b5f147
diff -r 7ea5ae2287a7 -r 6e28b992b99e main.cpp --- a/main.cpp Mon Oct 28 16:38:46 2019 +0000 +++ b/main.cpp Tue Oct 29 14:04:26 2019 +0000 @@ -84,6 +84,16 @@ //Hidscope HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application. +//State maschine +enum Motor_States{motor_wait , motor_cal1 , motor_cal2 , motor_encoderset}; +Motor_States motor_curr_state; +bool motor_state_changed = true; +bool motor_cal1_done = false; +bool motor_cal2_done = false; + +bool button1_pressed = false; +bool button2_pressed = false; + // PC connection MODSERIAL pc(USBTX, USBRX); @@ -93,6 +103,7 @@ Ticker directionTicker; Ticker encoderTicker; Ticker scopeTicker; +Ticker tickGlobal; //Global ticker const float PWM_period = 1e-6; @@ -106,9 +117,13 @@ float factorin = 6.23185/64; // Convert encoder counts to angle in rad float gearratio = 131.25; // Gear ratio of gearbox +void button1Press() +{ + button1_pressed = true; +} -// Vanaf hier is het misschien belangrijk +// Ticker Functions void readEncoder() { counts1 = encoder1.getPulses(); @@ -120,21 +135,37 @@ countsPrev2 = counts2; } -void motorCalibration1() -{ +void do_motorCalibration1() { + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + // Do stuff until end condition is met motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s float potmeter=potmeter1.read(); controlsignal1=0.0980f; motor1Power.write(abs(controlsignal1*motortoggle)); motor1Direction=0; - //Dit moet je doen zolang omega motor 1 > 0.iets - //State switch - //Dan motor 2 tot omega < 0.iets + + // State transition guard + if ( omega1 <= 0.5f ) { + motor_curr_state = motor_cal2; + motor_state_changed = true; + // More functions + } +} + +void do_motorCalibration2(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions } -void motorCalibration2(){ - + // Do stuff until end condition is met potmeter=potmeter1.read(); motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s @@ -148,13 +179,85 @@ motor2Power.write(abs(controlsignal2*motortoggle)); motor2Direction=1; //Dit moet je doen zolang omega van motor 2 > 0.iets + + // State transition guard + if ( omega2 <= 0.5f ) { + motor_curr_state = motor_encoderset; + motor_state_changed = true; + // More functions + } } +void do_motor_Encoder_Set(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + // Do stuff until end condition is met + + + // State transition guard + if ( omega2 <= 0.5f ) { + motor_curr_state = motor_encoderset; + motor_state_changed = true; + // More functions + } +} + +void do_motor_wait(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + // Do nothing until end condition is met + + // State transition guard + if ( button1_pressed ) { + button1_pressed = false; + motor_curr_state = motor_cal1; //Beginnen met calibratie + motor_state_changed = true; + // More functions + } +} + void toggleMotor() { motortoggle = !motortoggle; } +void motor_state_machine() +{ + switch(motor_curr_state) { + case motor_wait: + do_motor_wait(); + break; + case motor_cal1: + do_motorCalibration1(); + break; + case motor_cal2: + do_motorCalibration2(); + break; + case motor_encoderset: + do_motor_Encoder_Set(); + break; + } +} + +// Global loop of program +void tickGlobalFunc() +{ + //sampleSignal(); + //emg_state_machine(); + motor_state_machine(); + // controller(); + // outputToMotors(); +} + + int main() { pc.baud(115200); @@ -162,9 +265,8 @@ motor1Power.period(PWM_period); motor2Power.period(PWM_period); - motorTicker.attach(motorCalibration1, 0.01); //Dit moet je doen zolang omega < 0.iets - //motorTicker.attack(motorCalibration2, 0.01); - encoderTicker.attach(readEncoder, Ts); + motor_curr_state = motor_wait; // Start off in EMG Wait state + tickGlobal.attach( &tickGlobalFunc, Ts ); button2.fall(&toggleMotor);