Diff: main.cpp

Revision:

0:2386012c6594

Child:

1:b08ac32d1ddc

--- /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