![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
r
Dependencies: Encoder HIDScope TextLCD mbed-dsp mbed
Fork of Main-script_groep7 by
main.cpp
- Committer:
- phgbartels
- Date:
- 2014-10-30
- Revision:
- 4:377ddd65e4a6
- Parent:
- 3:156c3e536ed4
- Child:
- 5:4842219cb77c
File content as of revision 4:377ddd65e4a6:
/********************************************/ /* */ /* BRONCODE GROEP 7, MODULE 9, 2014 */ /* *-THE SLAP-* */ /* */ /* -Anne ten Dam */ /* -Laura de Heus */ /* -Moniek Strijdveen */ /* -Bart Arendshorst */ /* -Peter Bartels */ /********************************************/ /* #include voor alle libraries */ #include "TextLCD.h" #include "mbed.h" #include "encoder.h" //#include "HIDScope.h" #include "PwmOut.h" /* #define vaste waarden */ /*definieren pinnen Motor 1*/ #define M1_PWM PTA5 #define M1_DIR PTA4 #define M2_PWM PTC8 #define M2_DIR PTC9 /*Definieren minimale waarden PWM per motor*/ #define PWM1_min_50 0.529 /*met koppelstuk!*/ #define PWM2_min_30 0.529 /*aanpassen! Klopt nog niet met koppelstuk*/ /*Definieren om de hoeveel seconden er gegevens naar de HID-scope gestuurd worden.*/ #define TSAMP 0.005 #define K_P (100) #define K_I (50 * TSAMP) #define K_D (1) //#define K_D (0.0005 /TSAMP) #define I_LIMIT 100. #define PI 3.1415926535897 #define lengte_arm 0.5 /* Geef een naam aan een actie en vertel welke pinnen hiervoor gebruikt worden. */ TextLCD lcd(PTD2, PTA12, PTB2, PTB3, PTC2, PTA13, TextLCD::LCD16x2); // rs, e, d4-d7 Encoder motor1(PTD3,PTD1); Encoder motor2(PTD5, PTD0); PwmOut pwm_motor1(M1_PWM); PwmOut pwm_motor2(M2_PWM); DigitalOut motordir1(M1_DIR); DigitalOut motordir2(M2_DIR); DigitalOut LEDGREEN(LED_GREEN); DigitalOut LEDRED(LED_RED); Serial pc(USBTX,USBRX); /* definieer namen aan var, float, int, static float, uint8_t, uint16_t etc. en geef ze eventueel een waarde */ Ticker statemachine; Ticker screen; int previous_herhalingen = 0; int current_herhalingen = 0; int PWM2_percentage = 100; int aantal_rotaties_1 = 10; int aantalcr_1 = 1600; int aantalcr_2 = 960; int beginpos_motor1; int beginpos_motor1_new; int beginpos_motor2; int beginpos_motor2_new; int previous_pos_motor1 = 0; int current_pos_motor1; int delta_pos_motor1_puls; void clamp(float * in, float min, float max); volatile bool looptimerflag; int16_t gewenste_snelheid = 2; int16_t gewenste_snelheid_rad = 4; int16_t gewenste_snelheid_rad_return = 1; float pid(float setpoint, float measurement); float pos_motor1_rad; float PWM1_percentage = 0; float delta_pos_motor1_rad; float snelheid_motor1_rad; float snelheid_arm_ms; float PWM1; float PWM2; float Speed_motor1; float Speed_motor1rad; float setpoint = 0; float prev_setpoint = 0; //HIDScope scope(6); enum state_t {RUST, ARM_KALIBRATIE, EMG_KALIBRATIE, METEN_RICHTING, METEN_HOOGTE, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES state_t state = RUST; //functies die vanuit de statemachinefunction aangeroepen kunnen worden void rust() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } void arm_kalibratie() { //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; motor1.setPosition(0); motor2.setPosition(0); } void emg_kalibratie() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } void meten_richting() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } void meten_hoogte() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } void instellen_richting() { current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } void GotoPosition (float position_setpoint_rad, float speed_radpersecond) { current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is delta_pos_motor1_puls = current_pos_motor1 - previous_pos_motor1; //Bereken wat het verschil in waarde is voor het berekenen van de snelheid(puls) pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar delta_pos_motor1_rad = delta_pos_motor1_puls/(1600/(2*PI)); //delta waarde omrekenen naar rad voor snelheidsbepaling (rad) //snelheid_motor1_rad = delta_pos_motor1_rad / TSAMP; //rad delen door TSAMP om rad/s te verkrijgen //snelheid_arm_ms = snelheid_motor1_rad * lengte_arm; //deze waarde maar lengte van de arm om de snelheid van het uiteinde van die arm te verkrijgen //scope.set(0, snelheid_motor1_rad); previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde. //nu gaan we positie regelen i.p.v. snelheid. pc.printf("%f\n\r",pos_motor1_rad - position_setpoint_rad); if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.2) //if position error < ...rad, then stop. { speed_radpersecond = 0; setpoint = pos_motor1_rad; current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; pc.printf("stop\n\r"); } else if(pos_motor1_rad - position_setpoint_rad < 0) { setpoint = prev_setpoint +( TSAMP * speed_radpersecond); } else { setpoint = prev_setpoint -( TSAMP * speed_radpersecond); } /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/ PWM1_percentage = pid(setpoint, pos_motor1_rad); //scope.set(1, setpoint-pos_motor1_rad); if (PWM1_percentage < -100) { PWM1_percentage = -100; } else if (PWM1_percentage >100) { PWM1_percentage =100; } if(PWM1_percentage < 0) { motordir1 = 1; } else { motordir1 = 0; } pwm_motor1.write(abs(PWM1_percentage/100.));//abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50)); prev_setpoint = setpoint; //scope.send(); } float pid(float setpoint, float measurement) { float error; static float prev_error = 0; float out_p = 0; static float out_i = 0; float out_d = 0; error = (setpoint-measurement); out_p = error*K_P; out_i += error*K_I; out_d = (error-prev_error)*K_D; clamp(&out_i,-I_LIMIT,I_LIMIT); prev_error = error; //scope.set(2, out_p); //scope.set(3, out_i); //scope.set(4, out_d); return out_p + out_i + out_d; } void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variebele instaat). Dus je slaat niet de variabele op // maar de locatie van de variabele. { *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c // *in = het getal dat staat op locatie van in --> waarde van new_pwm } void statemachinefunction() { switch(state) { case RUST: { rust(); /*voorwaarde wanneer hij door kan naar de volgende case*/ if (current_herhalingen == 600) { current_herhalingen = 0; previous_herhalingen = 0; state = ARM_KALIBRATIE; } break; /*bepalen hoe je zorgt dat je maar 1x de kalibraties uitvoerd! (Of eventueel de arm kalibratie elke keer!!)*/ //if (metingstatus<5); // state = ARMKALIBRATIE; //if (metingstatus==5); // state = METEN_RICHTING; //break; //} } case ARM_KALIBRATIE: { arm_kalibratie(); if (current_herhalingen == 200) { current_herhalingen = 0; previous_herhalingen = 0; state = EMG_KALIBRATIE; motor1.setPosition(0); motor2.setPosition(0); pwm_motor1.period_us(100); pwm_motor2.period_us(100); } break; } case EMG_KALIBRATIE: { emg_kalibratie(); if (current_herhalingen == 200) { state = METEN_RICHTING; current_herhalingen = 0; previous_herhalingen = 0; } break; } case METEN_RICHTING: { meten_richting(); if (current_herhalingen == 200) { state = METEN_HOOGTE; current_herhalingen = 0; previous_herhalingen = 0; } break; } case METEN_HOOGTE: { meten_hoogte(); if (current_herhalingen == 200) { state = INSTELLEN_RICHTING; current_herhalingen = 0; previous_herhalingen = 0; } break; } case INSTELLEN_RICHTING: { instellen_richting(); if (current_herhalingen == 200) { state = SLAAN; current_herhalingen = 0; previous_herhalingen = 0; } break; } case SLAAN: { GotoPosition(0.7,4); if (current_herhalingen == 200) { state = RETURN2RUST; current_herhalingen = 0; previous_herhalingen = 0; prev_setpoint =0; setpoint =0; } break; } case RETURN2RUST: { GotoPosition(0,1); /*if (current_herhalingen == 200) { state = RUST; current_herhalingen = 0; previous_herhalingen = 0; } */ break; } default: { state = RUST; } }//switch(state) }//void statemachinefunction void screenupdate(){ if(state==RUST){ lcd.cls(); lcd.locate(0,0); lcd.printf("S.T.I.E.N.E.N."); //regel 1 LCD scherm lcd.locate(0,1); lcd.printf(" GROEP 7 "); } else{ lcd.cls(); lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen. } } int main() { pc.baud(115200); 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); }