Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
Diff: main.cpp
- Revision:
- 13:95a4bb9daf63
- Parent:
- 12:3fec73bc3318
- Child:
- 14:b1de410384c2
--- a/main.cpp Mon Nov 03 15:15:31 2014 +0000 +++ b/main.cpp Mon Nov 03 21:31:16 2014 +0000 @@ -26,6 +26,8 @@ /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/ #define TSAMP 0.005 #define K_P (80000) +#define K_P_motor2 (75) +#define K_D_motor2 (0.01) #define K_I (0.01) #define K_D (0.01) #define I_LIMIT 100. @@ -62,6 +64,7 @@ int current_herhalingen_1 = 0; int previous_herhalingen_1 = 0; int previous_pos_motor1 = 0; +int previous_pos_motor2 = 0; int current_pos_motor1; int current_pos_motor2; int EMG = 1; @@ -69,14 +72,17 @@ int doel; int doel_richting; int doel_hoogte; -int puls_richting1; +int puls_richting1 = 4000; int puls_richting2; -int puls_richting3; +int puls_richting3 = 4000; bool aanspan; void clamp(float * in, float min, float max); float pid(float setpoint, float measurement); +float pid_motor2(float setpoint, float measurement); float pos_motor1_rad; +float pos_motor2_rad; float PWM1_percentage = 0; +float PWM2_percentage = 0; float PWM1; float PWM2; float setpoint = 0; @@ -97,8 +103,9 @@ float PWM_richting1; float PWM_richting2; float PWM_richting3; +float position2_setpoint; -enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES +enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES state_t state = RUST; void rust() @@ -173,9 +180,7 @@ { //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop motor1.setPosition(0); - motor2.setPosition(0); - pwm_motor1.period_us(100); - pwm_motor2.period_us(100); + //motor2.setPosition(0); akkoord_geven(); }//void arm_kalibratie @@ -214,68 +219,8 @@ doel_bepaling(); }//void meten_richting -void instellen_richting() -{ - current_pos_motor2 = motor2.getPosition(); - pc.printf("%d\n\r", current_pos_motor2); - /* - if (doel_richting ==1){ - if (state==RETURN2RUST){ - motordir2 = 1; - while (current_pos_motor2 > 0){ - pwm_motor2.write(PWM_richting1); - }//while (current_pos_motor2 < rad_richting1) - current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; - }//if (doel_richting == 1) - else{ - motordir2 = 0; - while (current_pos_motor2 < puls_richting1){ - pwm_motor2.write(PWM_richting1); - }//while (current_pos_motor2 < rad_richting1) - current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; - }//else - }//if (doel_richting ==1) - - - else if (doel_richting == 2){ - if (state==RETURN2RUST){ - motordir2 = 1; - while (current_pos_motor2 > 0){ - pwm_motor2.write(PWM_richting2); - }//while (current_pos_motor2 < rad_richting1) - current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; - }//if (doel_richting == 1) - else{ - motordir2 = 0; - while (current_pos_motor2 < puls_richting2){ - pwm_motor2.write(PWM_richting2); - }//while (current_pos_motor2 < rad_richting1) - current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; - }//if (doel_richting == 1) - }//if (doel_richting ==1) - - else if(doel_richting == 3){ - if (state==RETURN2RUST){ - motordir2 =1; - while (current_pos_motor2 > 0){ - pwm_motor2.write(PWM_richting3); - }//while (current_pos_motor2 < rad_richting1) - current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; - }//if (doel_richting == 1) - else{ - motordir2 = 0; - while (current_pos_motor2 < puls_richting3){ - pwm_motor2.write(PWM_richting3); - }//while (current_pos_motor2 < rad_richting1) - current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1; - }//if (doel_richting == 1) - }//if (doel_richting ==1) - */ -}//void instellen_richting - void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge) { - current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. @@ -301,10 +246,9 @@ if (PWM1_percentage < -100) { PWM1_percentage = -100; - }//if - else if (PWM1_percentage >100) { + } else if (PWM1_percentage >100) { PWM1_percentage =100; - }//else if + } if(PWM1_percentage < 0) { motordir1 = 1; @@ -333,6 +277,60 @@ return out_p + out_i + out_d; }//float pid +void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2) +{ + if (setpoint < position2_setpoint_rad) { + setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond); + if (setpoint > position2_setpoint_rad) { + setpoint = position2_setpoint_rad; + } + } + + else if (setpoint > position2_setpoint_rad) { + setpoint = prev_setpoint - (0.0005 * speed2_radpersecond); + if (setpoint < position2_setpoint_rad) { + setpoint = position2_setpoint_rad; + } + } else if (setpoint == position2_setpoint_rad) { + current_herhalingen = previous_herhalingen + 1; + previous_herhalingen = current_herhalingen; + } + + current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is + pc.printf("c: %d\r\n", motor2.getPosition());//current_pos_motor2); + pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa + PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad); + + if (PWM2_percentage < -100) { + PWM2_percentage = -100; + } else if (PWM2_percentage >100) { + PWM2_percentage =100; + } + + if(PWM2_percentage < 0) { + motordir2 = 1; + }//if + else { + motordir2 = 0; + }//else + + pwm_motor2.write(abs(PWM2_percentage/100.)); + prev_setpoint = setpoint; +} + +float pid_motor2(float setpoint, float measurement) +{ + float error; + static float prev_error = 0; + float out_p = 0; + float out_d = 0; + error = (setpoint-measurement); + out_p = error*K_P_motor2; + out_d = (error-prev_error)*K_D_motor2; + prev_error = error; + return out_p + out_d; +}//float pid + void clamp(float* in, float min, float max) { *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; @@ -340,7 +338,6 @@ void statemachinefunction() { - pc.printf("_%d_",statetimer.read_us()); statetimer.reset(); //pc.printf("."); switch(state) { @@ -424,17 +421,31 @@ break; }//case METEN_RICHTING + + case TEST: { + state = RUST; + break; + } + case INSTELLEN_RICHTING: { - instellen_richting(); - - /*if (current_herhalingen == 100) - { - current_herhalingen_1 = 0; - previous_herhalingen_1 = 0; + //instellen_richting(); + if (doel_richting == 1) { + position2_setpoint = 0; + } else if (doel_richting ==2) { + position2_setpoint = 14.5; + } else { + position2_setpoint = 25; + } + + + translatie(position2_setpoint, 10, 0.1); + + if (current_herhalingen >= 400) { + current_herhalingen = 0; + previous_herhalingen = 0; doel_richting = 0; - state = SLAAN; - }//if (current_herhalingen == 100) - */ + state = RETURN2RUST; + }//if (current_herhalingen == 100 break; }//case INSTELLEN_RICHTING @@ -451,8 +462,8 @@ }//case SLAAN case RETURN2RUST: { - instellen_richting(); - GotoPosition(0,4, 0.05); + translatie(0,10,0.1); + /*GotoPosition(0,4, 0.05); doel_richting = 0; doel_hoogte = 0; if (current_herhalingen >= 200 && current_herhalingen_1 >= 200) { @@ -462,6 +473,7 @@ current_herhalingen = 0; current_herhalingen = 0; }//if (current_herhalingen == 100) + */ break; }// case RETURN2RUST @@ -475,7 +487,6 @@ void screenupdate() { - pc.printf("l"); if(state==RUST) { lcd.cls(); lcd.locate(0,0); @@ -611,13 +622,11 @@ }//else if(state==METEN_RICHTING){ else if(state==INSTELLEN_RICHTING) { - pc.printf("i"); lcd.cls(); lcd.locate(0,0); lcd.printf("Instellen hoek"); lcd.locate(0,1); lcd.printf("Even wachten..."); - pc.printf("e"); }//else if(state==INSTELLEN_RICHTING){ else if(state==SLAAN) { @@ -644,16 +653,18 @@ int main() { + pwm_motor1.period_us(100); + pwm_motor2.period_us(100); pc.baud(115200); statetimer.start(); arm_biquad_cascade_df1_init_f32(&lowpass_1,1 , lowpass_1_const, lowpass_1_states); arm_biquad_cascade_df1_init_f32(&highpass,1 , highpass_const, highpass_states); arm_biquad_cascade_df1_init_f32(¬ch,1 , notch_const, notch_states); arm_biquad_cascade_df1_init_f32(&lowpass_2,1 , lowpass_2_const, lowpass_2_states); + state = TEST; statemachine.attach(&statemachinefunction, TSAMP); // the address of the function to be attached (flip) and the interval (2 seconds) //screen.attach(&screenupdate, 0.2); - while(1) - { + while(1) { screenupdate(); wait(0.2); };