Laura Heus
Alleen display nog goed instellen
Dependencies: Encoder HIDScope TextLCD mbed-dsp mbed
Fork of
Main-script_groep7
by 
Laura Heus
Revision 0:2386012c6594, committed 2014-10-29
- Comitter:
- phgbartels
- Date:
- Wed Oct 29 16:02:12 2014 +0000
- Child:
- 1:b08ac32d1ddc
- Commit message:
- Op dit moment zitten er geen fouten in het script. De motor doet nog niets, ik vermoed dat er iets fout zit in het tellen --> ik wilde het tellen dus graag uitzetten! (in alle cases, zodat hij gelijk naar de motor springt!)
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.lib Wed Oct 29 16:02:12 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Daanmk/code/Encoder/#9a8b76f0908c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HIDScope.lib Wed Oct 29 16:02:12 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/tomlankhorst/code/HIDScope/#e44574634162
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Wed Oct 29 16:02:12 2014 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/simon/code/TextLCD/#308d188a2d3a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Oct 29 16:02:12 2014 +0000
@@ -0,0 +1,335 @@
+/********************************************/
+/* */
+/* 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);
+
+/*
+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;
+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;
+uint16_t gewenste_snelheid = 2;
+uint16_t gewenste_snelheid_rad = 4;
+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;
+
+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);
+ pwm_motor1.period_us(100);
+ pwm_motor2.period_us(100);
+
+}
+
+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 slaan () {
+ float setpoint;
+ float prev_setpoint = 0;
+ 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.
+ if (current_pos_motor1 >= 400)
+ {
+ gewenste_snelheid_rad = 0;
+ }
+
+ setpoint = prev_setpoint + TSAMP * gewenste_snelheid_rad;
+ /*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;
+ }
+ else {}
+
+ if(PWM1_percentage < 0)
+ {
+ motordir1 = 1;
+ }
+ else
+ {
+ motordir1 = 0;
+ }
+
+ pwm_motor1.write(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 return2rust () {
+}
+
+void statemachinefunction()
+{
+ switch(state) {
+ case RUST: {
+ rust();
+ /*voorwaarde wanneer hij door kan naar de volgende case*/
+ if (current_herhalingen == 200)
+ {
+ state = ARM_KALIBRATIE;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ 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)
+ {
+ state = EMG_KALIBRATIE;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ 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:
+ {
+ slaan();
+ if (current_herhalingen == 200)
+ {
+ state = RETURN2RUST;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ break;
+ }
+ }
+
+ case RETURN2RUST:
+ {
+ return2rust();
+ 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() {
+ statemachine.attach(&statemachinefunction, 0.005); // the address of the function to be attached (flip) and the interval (2 seconds)
+ screen.attach(&screenupdate, 0.2);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Oct 29 16:02:12 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/cb3d968589d8 \ No newline at end of file