Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 4:1e8da6b5f147
- Parent:
- 3:6e28b992b99e
- Child:
- 5:4d8b85b7cfc4
diff -r 6e28b992b99e -r 1e8da6b5f147 main.cpp --- a/main.cpp Tue Oct 29 14:04:26 2019 +0000 +++ b/main.cpp Tue Oct 29 15:32:04 2019 +0000 @@ -26,7 +26,7 @@ float motor1angle; //Measured angle motor 1 float motor2angle; //Measured angle motor 2 float potmeter; -float omega1=1; //velocity rad/s motor 1 +float omega1; //velocity rad/s motor 1 float omega2; //Velocity rad/s motor2 float deg2rad=0.0174532; //Conversion factor degree to rad float rad2deg=57.29578; //Conversion factor rad to degree @@ -85,7 +85,7 @@ 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}; +enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_encoderset}; Motor_States motor_curr_state; bool motor_state_changed = true; bool motor_cal1_done = false; @@ -135,6 +135,29 @@ countsPrev2 = counts2; } +void do_motor_cal1_start(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + //Do stuff + motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad + omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s + controlsignal1=0.10f; + motor1Power.write(abs(controlsignal1*motortoggle)); + motor1Direction=0; + + // State transition guard + if ( abs(omega1) >= 1.5f ){ + motor_curr_state = motor_cal1; + motor_state_changed = true; + // More functions + } + +} + void do_motorCalibration1() { // Entry function if ( motor_state_changed == true ) { @@ -151,13 +174,15 @@ motor1Direction=0; // State transition guard - if ( omega1 <= 0.5f ) { - motor_curr_state = motor_cal2; + if ( abs(omega1) <= 0.5f ) { + motor_curr_state = motor_cal2_start; motor_state_changed = true; // More functions } } +void do_motor_cal2_start() + void do_motorCalibration2(){ // Entry function if ( motor_state_changed == true ) { @@ -218,7 +243,7 @@ // State transition guard if ( button1_pressed ) { button1_pressed = false; - motor_curr_state = motor_cal1; //Beginnen met calibratie + motor_curr_state = motor_cal1_start; //Beginnen met calibratie motor_state_changed = true; // More functions } @@ -235,9 +260,15 @@ case motor_wait: do_motor_wait(); break; + case motor_cal1_start: + do_motor_cal1_start(); + break; case motor_cal1: do_motorCalibration1(); break; + case motor_cal2_start: + do_motor_cal2_start(); + break; case motor_cal2: do_motorCalibration2(); break; @@ -253,6 +284,7 @@ //sampleSignal(); //emg_state_machine(); motor_state_machine(); + readEncoder(); // controller(); // outputToMotors(); } @@ -268,11 +300,12 @@ motor_curr_state = motor_wait; // Start off in EMG Wait state tickGlobal.attach( &tickGlobalFunc, Ts ); - button2.fall(&toggleMotor); + button1.fall(&button1Press); while (true) { potmeter=potmeter1.read(); pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1); + pc.printf("Currentstate: %i motor_cal1: %i\r\n",motor_curr_state, motor_cal1); wait(0.5); } }