Diff: main.cpp

Revision:

1:b08ac32d1ddc

Parent:

0:2386012c6594

Child:

2:de7b6c1d67c4

--- a/main.cpp	Wed Oct 29 16:02:12 2014 +0000
+++ b/main.cpp	Thu Oct 30 09:00:36 2014 +0000
@@ -16,7 +16,7 @@
 #include "TextLCD.h"
 #include "mbed.h"
 #include "encoder.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 #include "PwmOut.h"
 
 /*
@@ -59,7 +59,7 @@
 Ticker statemachine;
 Ticker screen;
 int previous_herhalingen = 0;
-int current_herhalingen;
+int current_herhalingen = 0;
 int PWM2_percentage = 100; 
 int aantal_rotaties_1 = 10;
 int aantalcr_1 = 1600;
@@ -85,8 +85,10 @@
 float PWM2;
 float Speed_motor1;
 float Speed_motor1rad;
+float setpoint = 0;
+float prev_setpoint = 0; 
 
-HIDScope scope(6);
+//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;
@@ -100,10 +102,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
     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);
-    
+    motor2.setPosition(0);    
 }
 
 void emg_kalibratie() {
@@ -123,15 +122,13 @@
 }
 
 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);
+    //scope.set(0, snelheid_motor1_rad);
     
     previous_pos_motor1 = current_pos_motor1;  //sla de huidige waarde op als vorige waarde.
     
@@ -144,7 +141,7 @@
     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); 
+    //scope.set(1, setpoint-pos_motor1_rad); 
     
     if (PWM1_percentage < -100)
     {
@@ -166,9 +163,9 @@
     }
         
     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));
+    //scope.set(5, abs(((1-PWM1_min_50)/100)*PWM1_percentage + PWM1_min_50));
     prev_setpoint = setpoint;
-    scope.send();   
+    //scope.send();   
 }
 
 float pid(float setpoint, float measurement)
@@ -184,9 +181,9 @@
     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);
+    //scope.set(2, out_p);
+    //scope.set(3, out_i);
+    //scope.set(4, out_d);
     return out_p + out_i + out_d;
 }
 
@@ -206,14 +203,13 @@
         case RUST: {
             rust();
             /*voorwaarde wanneer hij door kan naar de volgende case*/
-            if (current_herhalingen == 200) 
+            if (current_herhalingen == 600) 
             {
-                state = ARM_KALIBRATIE;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
+                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;
@@ -228,11 +224,11 @@
             arm_kalibratie();
             if (current_herhalingen == 200) 
             {
-                state = EMG_KALIBRATIE;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
+                state = EMG_KALIBRATIE;
             }
+            break;
         }
 
         case EMG_KALIBRATIE: 
@@ -243,8 +239,8 @@
                 state = METEN_RICHTING;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
             }
+            break;
         }
 
         case METEN_RICHTING: 
@@ -255,8 +251,8 @@
                 state = METEN_HOOGTE;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
             }
+            break;
         }
 
         case METEN_HOOGTE: 
@@ -267,8 +263,8 @@
                 state = INSTELLEN_RICHTING;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
             }
+            break;
         }
 
         case INSTELLEN_RICHTING: 
@@ -279,8 +275,8 @@
                 state = SLAAN;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
             }
+            
         }
 
         case SLAAN: 
@@ -291,8 +287,8 @@
                 state = RETURN2RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
             }
+            break;
         }
 
         case RETURN2RUST: 
@@ -303,8 +299,8 @@
                 state = RUST;
                 current_herhalingen = 0;
                 previous_herhalingen = 0;
-                break;
             }
+            break;
         }
         
         default: {
@@ -330,6 +326,10 @@
 }
 
 int main() {
+    motor1.setPosition(0);
+    motor2.setPosition(0);
+    pwm_motor1.period_us(100);
+    pwm_motor2.period_us(100);
     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