Diff: main.cpp

Revision:

1:e5e1eb9d0025

Parent:

0:859c89785d3f

Child:

2:0cb899f2800a

--- a/main.cpp	Wed Oct 15 12:40:36 2014 +0000
+++ b/main.cpp	Wed Oct 15 14:37:28 2014 +0000
@@ -4,12 +4,19 @@
 #include "TextLCD.h"        //LCD scherm bibliotheek inladen, communicatie met LCD scherm
 
 //Constanten definiëren en waarde geven
-
+#define SAMPLETIME_REGELAAR     0.005
+#define KP                      1
 
 //Pinverdeling en naamgeving variabelen
 TextLCD         lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2);      //LCD scherm
 MODSERIAL       pc(USBTX, USBRX);                               //PC
 
+PwmOut pwm_motor_arm1(PTA5);            //PWM naar motor arm 1
+DigitalOut dir_motor_arm1(PTD1);        //Richting van motor arm 1
+Encoder puls_motor_arm1(PTD0, PTC9);    //Encoder pulsen tellen van motor arm 1
+
+Ticker ticker_regelaar;                  
+
 //Gedefinieerde datatypen en naamgeving
 bool rust = false;                      //Bool voor controleren volgorde in programma
 bool kalibratie_positie = false;        //Bool voor controleren volgorde in programma
@@ -18,6 +25,16 @@
 char *lcd_r1 = new char[16];             //Char voor tekst op eerste regel LCD scherm
 char *lcd_r2 = new char[16];             //Char voor tekst op tweede regel LCD scherm
 
+volatile bool regelaar_ticker_flag;
+void setregelaar_ticker_flag(){
+    regelaar_ticker_flag = true;
+}
+
+void keep_in_range(float * in, float min, float max);
+
+int puls_arm1_home = 363;
+float pwm_to_motor_arm1;
+
 //Beginwaarden voor variabelen
 
 
@@ -34,4 +51,32 @@
         pc.printf("Aanzetten compleet");    //Tekst voor controle Aanzetten
         rust = true;                        //Rust wordt waar zodat door kan worden gegaan naar het volgende deel
     }
+    
+    
+    
+    //Arm 1 naar thuispositie
+    if (rust == true && kalibratie_positie == false && kalibratie_EMG == false){
+        wait(1);                            //Een seconde wachten
+        
+        ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR);       //Ticker iedere zoveel seconde de flag op laten steken
+        
+        while(1) {
+            while(regelaar_ticker_flag != true) ;
+            regelaar_ticker_flag = false;
+            
+            pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP;
+            keep_in_range(&pwm_to_motor_arm1, -1, 1);
+            
+            if (pwm_to_motor_arm1 > 0){
+                dir_motor_arm1.write(1);
+            }
+            else {
+                dir_motor_arm1.write(0);
+            }
+            pwm_motor_arm1.write(abs(pwm_to_motor_arm1));
+                    
+            wait(1);
+            pc.printf("Arm 1 naar thuispositie compleet");
+        }   
+    }    
 }
\ No newline at end of file