Diff: PROJECT_main.cpp

Revision:

10:6bf3e25f020a

Parent:

9:0bc7f83b761e

Child:

11:b517e73a98ab

--- a/PROJECT_main.cpp	Mon Nov 03 20:17:43 2014 +0000
+++ b/PROJECT_main.cpp	Mon Nov 03 21:31:40 2014 +0000
@@ -13,7 +13,7 @@
 
 //Define objects
 
-HIDScope scope(2);
+HIDScope scope(4);
 
 AnalogIn    emg0(PTB1);         //Analog input biceps
 AnalogIn    emg1(PTB2);         //Analog input triceps
@@ -134,10 +134,16 @@
 void setlooptimerflag(void)
 {
     looptimerflag = true;
+}
+
+Ticker hid; 
+
+void hidscope(void){
     scope.set(0, motor2.getPosition()*omrekenfactor2);
-    scope.set(0, motor1.getPosition()*omrekenfactor1);
-    scope.send();
-
+    scope.set(1, setpoint2);
+    scope.set(2, motor1.getPosition()*omrekenfactor1); 
+    scope.set(3, setpoint1);  
+    scope.send();    
 }
 
 void keep_in_range(float * in, float min, float max)
@@ -272,6 +278,7 @@
 
 int main()
 {
+    hid.attach(hidscope, 0.01); 
     pc.baud(115200);                        //baudrate instellen
     log_timer.attach(looper, TSAMP_EMG);    //EMG, Fsample 500 Hz
     looptimer.attach(setlooptimerflag,TSAMP);
@@ -526,6 +533,8 @@
     setpoint2=0;
     integral1 = integral = 0;
     previouserror1 = previouserror = 0;
+    
+    
     while(1) {      // loop voor het goed plaatsen van motor2 (batje hoek)
         while(!looptimerflag)
             {}
@@ -547,11 +556,13 @@
                         break;
                 }                
                 if(setpoint2>angle) { //abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.1
-                    setpoint2 = angle;                   
+                    setpoint2 = angle;   
+                    count = 0;
+                    state=2;                
                 }
-                if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.04) {
+                /*if(abs(setpoint2-motor2.getPosition()*omrekenfactor2) < 0.02) {
                     state = 2;    
-                }
+                }*/
                 break;
 
             case 2:
@@ -561,10 +572,11 @@
                     count = 0;
                     state = 3;
                 }
+                break;
             case 3:
                 switch (force) {
                     case 1:
-                        setpoint1 += 0.4*TSAMP; //6.8*TSAMP;
+                        setpoint1 += 2.5*TSAMP; //6.8*TSAMP;
                         break;
                     case 2:
                         setpoint1 += 0.4*TSAMP; //7.4*TSAMP;
@@ -573,18 +585,18 @@
                         setpoint1 += 0.4*TSAMP; //8.0*TSAMP;
                         break;
                 }
-                if(abs(motor1.getPosition()*omrekenfactor1)>2.1) {
+                if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) {
                     state = 4;
                 }
                 break;
             case 4:
                 setpoint2 -= 0.25*TSAMP;
-                if(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) { // op 0 draait hij mogelijk in de arm
+                if(setpoint2 < 0.001) { //(abs(setpoint2 - motor2.getPosition()*omrekenfactor2) < 0.1) - op 0 draait hij mogelijk in de arm
                     state = 5;
                 }
                 break;
             case 5:
-                setpoint1 -= 0.25*TSAMP;
+                setpoint1 -= 0.5*TSAMP;
                 if(setpoint1 < 0) {
                     state = 6;
                 }
@@ -610,7 +622,7 @@
         previouserror1 = controlerror1;
 
         keep_in_range(&pwm1, -1,1);
-        pwm_motor1.write(abs(pwm1));
+        pwm_motor1.write(fabs(pwm1));
         if(pwm1 > 0) {
             motor1dir = 1;
         } else {
@@ -626,7 +638,7 @@
         previouserror = controlerror;
 
         keep_in_range(&pwm, -1,1);
-        pwm_motor2.write(abs(pwm));
+        pwm_motor2.write(fabs(pwm));
         if(pwm > 0) {
             motor2dir = 1;
         } else {