Diff: PROJECT_main.cpp

Revision:

16:a0a39512bd47

Parent:

11:b517e73a98ab

Child:

17:7641d7934b91

--- a/PROJECT_main.cpp	Mon Nov 03 22:16:30 2014 +0000
+++ b/PROJECT_main.cpp	Tue Nov 04 08:05:21 2014 +0000
@@ -13,7 +13,7 @@
 
 //Define objects
 
-HIDScope scope(5);
+HIDScope scope(6);
 
 AnalogIn    emg0(PTB1);         //Analog input biceps
 AnalogIn    emg1(PTB2);         //Analog input triceps
@@ -524,8 +524,6 @@
 
     /* Vanaf hier komt de aansturing van de motor */
 
-
-// FORMAT_CODE_START
     setpoint1=0;
     setpoint2=0;
     integral1 = integral = 0;
@@ -535,14 +533,6 @@
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag);
         looptimerflag = false; //clear flag
-        setpoint1=0;
-        setpoint2=0;
-        integral1 = integral = 0;
-        previouserror1 = previouserror = 0;
-
-
-       
-// FORMAT_CODE_START
 
             scope.set(0, motor2.getPosition()*omrekenfactor2);
             scope.set(1, setpoint2);
@@ -597,6 +587,7 @@
                             break;
                     }
                     if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
+                        setpoint1 = 2.36; 
                         state = 4;
                     }
                     break;
@@ -635,7 +626,8 @@
             derivative1 = (controlerror1 - previouserror1)/TSAMP;
             pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1;
             previouserror1 = controlerror1;
-
+            scope.set(5, pwm1);
+            
             keep_in_range(&pwm1, -1,1);
             pwm_motor1.write(fabs(pwm1));
             if(pwm1 > 0) {