Diff: main.cpp

Revision:

8:d4161e9be1da

Parent:

7:dd3cba61b34b

--- a/main.cpp	Fri Oct 17 07:52:22 2014 +0000
+++ b/main.cpp	Mon Oct 20 11:56:38 2014 +0000
@@ -5,19 +5,19 @@
 
 //Constanten definiëren en waarde geven
 #define SAMPLETIME_REGELAAR     0.005       //Sampletijd ticker regelaar motor
-#define KP                      1           //Factor proprotionele regelaar
+#define KP                      0.001           //Factor proprotionele regelaar
 #define SAMPLETIME_EMG          0.005       //Sampletijd ticker EMG meten
 
 //Pinverdeling en naamgeving variabelen
-TextLCD         lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2);      //LCD scherm
+TextLCD         lcd(PTD3, 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
-PwmOut pwm_motor_arm2(PTA12);           //PWM naar motor arm 2
-DigitalOut dir_motor_arm2(PTD3);        //Ricting van motor arm 2
-Encoder puls_motor_arm2(PTD4, PTC8);    //Encoder pulsen tellen van motor arm 2
+PwmOut pwm_motor_arm1(PTC8);            //PWM naar motor arm 1
+DigitalOut dir_motor_arm1(PTC9);        //Richting van motor arm 1
+Encoder puls_motor_arm1(PTD0, PTD2);    //Encoder pulsen tellen van motor arm 1
+PwmOut pwm_motor_arm2(PTA5);            //PWM naar motor arm 2
+DigitalOut dir_motor_arm2(PTA4);        //Ricting van motor arm 2
+Encoder puls_motor_arm2(PTD5, PTA13);   //Encoder pulsen tellen van motor arm 2
 AnalogIn EMG_bi(PTB1);                  //Meten EMG signaal biceps
 
 
@@ -68,14 +68,15 @@
     keep_in_range(&pwm_to_motor_arm1, -1, 1);                                       //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
             
     if (pwm_to_motor_arm1 > 0){                     //Als PWM is positief, dan richting 1
-       dir_motor_arm1.write(1);
+        dir_motor_arm1.write(1);
     }
     else {                                          //Anders richting nul
         dir_motor_arm1.write(0);
     }
     pwm_motor_arm1.write(abs(pwm_to_motor_arm1));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+    pc.printf("pwm_to_motor_arm1 is %d\n\r",pwm_to_motor_arm1);
             
-    if (pwm_to_motor_arm1 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
+    if (pwm_to_motor_arm1 == 0){                        //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
         kalibratie_positie_arm1 = true;
         pc.printf("Arm 1 naar thuispositie compleet");     //Tekst voor controle Arm 1 naar thuispositie
     }  
@@ -92,6 +93,7 @@
         dir_motor_arm2.write(0);
     }
     pwm_motor_arm2.write(abs(pwm_to_motor_arm2));   //Output PWM naar motor is de absolute waarde van de berekende PWM
+    pc.printf("pwm_to_motor_arm2 is %f\n\r",puls_motor_arm2.getPosition());
             
     if (pwm_to_motor_arm2 == 0) {                           //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
         kalibratie_positie_arm2 = true;
@@ -130,7 +132,7 @@
         }   
         wait(1);                    //Een seconde wachten
     }
-    
+    pc.printf("hoi");
     //Arm 2 naar thuispositie
     if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){
         wait(1);                            //Een seconde wachten