Diff: main.cpp

Revision:

55:4eb229a35859

Parent:

54:2b54283b3b47

Child:

56:29c31b83c4ce

--- a/main.cpp	Mon Nov 03 12:06:11 2014 +0000
+++ b/main.cpp	Mon Nov 03 15:09:53 2014 +0000
@@ -9,11 +9,11 @@
 #define K_I (0.02  *TSAMP1)
 #define K_D (0  /TSAMP1)
 #define I_LIMIT 1.
-#define K_Pp (0.0025) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+#define K_Pp (0.003) //-------------------------------Deze drie waarden moeten anders!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 #define K_Ip (0.0  *TSAMP2)//-----------------------------TSAMP1 veranderd naar TSAMP2
-#define K_Dp (0.000004  /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
+#define K_Dp (0.0000003  /TSAMP2)//--------------------------------TSAMP1 veranderd naar TSAMP2
 
-#define TSAMP1 0.01
+#define TSAMP1 0.025
 #define TSAMP2 0.01
 #define WACHTEN 1
 #define SLAAN 2
@@ -35,7 +35,7 @@
 //alle initiaties voor EMG
 MODSERIAL pc(USBTX,USBRX);
 
-HIDScope scope(4);
+HIDScope scope(6);
 
 AnalogIn emgB(PTB1); //biceps
 AnalogIn emgT(PTB2); //tricep
@@ -85,10 +85,10 @@
 float new_pwm;
 float PWM2 = 0.3; //PWM voor instellen hoek batje, kan waarschijnlijk een stuk langzamer
 int toestand = WACHTEN; //terugkeren?
-float setspeed = 0, V3=60, V2=40, V1 =30;//V in counts/s
+float setspeed = 0, V3=80, V2=40, V1 =30;//V in counts/s
 
 
-Encoder motor1(PTD5,PTD3);
+Encoder motor1(PTD5,PTD3, true);
 Encoder motor2(PTD0,PTD2);
 
 DigitalOut motordir1(PTA4);
@@ -357,9 +357,9 @@
             looptimer2.attach(motor2aansturing,TSAMP1);
             wait(8);
             looptimer2.detach();
-            pc.printf("Detach Motor 1\n"); */
+            pc.printf("Detach Motor 1\n");
 
-//------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
+            //------------------------------------------------------------------------------------------------------------------------------- eind aansturing motor 2
             wait(2);
             /* Ticker log_timerB;
 
@@ -429,16 +429,16 @@
             looptimer1.attach(motor1aansturing,TSAMP1);
             wait(2); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer1.detach();
-            pc.printf("detachMotor1\n"); 
+            pc.printf("detachMotor1\n");
 
             pid(0,0,true);
-                        
+
             Ticker looptimer3;
             looptimer3.attach(motor1aansturingdeel2,TSAMP1);  //of TSAMP2?....
-            wait(5); ////is aan te passen (tijd die nodig is om balletje te slaan
+            wait(4); ////is aan te passen (tijd die nodig is om balletje te slaan
             looptimer3.detach();
             pc.printf("detachMotor1\n");
-            
+
             pidpositie(0,0,true);
             pwm_motor1.write(0);
             toestand = WACHTEN; //hierheen verplaatst vanaf motor1aansturingdeel2. Belangrijk!! niet weghalen!!
@@ -448,7 +448,7 @@
             myled3=1;
             groen=0;
             blauw=0;
-            
+
         }
     }
     //}
@@ -706,9 +706,10 @@
                 pwm_motor1.write(0.0);//arvid had hier 0,0 gezet?!
                 pc.printf("toestand = terugkeren, wacht tot 2e ticker\n\r");
                 //stop = 1;
-            }
 
+            }
             break;
+
         case WACHTEN:
             pidpositie(ANGLEMIN,motor1.getPosition()); //dit is nu alleen een waarde, moet vervolgens in een functie terugkomen toch
             //?? motor1.getPosition(nieuwe positie);
@@ -733,6 +734,7 @@
                 }//end switch
             } //end if
             break;
+
             /*case TERUGKEREN:
                 if (motor1.getPosition()<=ANGLEMAX) {
                     new_pwmpos = pidpositie(ANGLEMAX, motor1.getPosition());
@@ -747,7 +749,9 @@
                 break;*///overbodig!!!
     } //end switch
     scope.set(0,motor1.getPosition());
-    scope.set(1,motor1.getPosition());
+    scope.set(1,ANGLEMAX);
+    scope.set(4,motor1.getSpeed());
+    scope.set(5, setspeed);
     scope.send();
 }
 
@@ -770,6 +774,8 @@
             break;
     } //end switch
     scope.set(0,motor1.getPosition());        //ruwe data
-    scope.set(2,motor1.getPosition());     //filtered...
+    scope.set(2,ANGLEMIN);
+    scope.set(4,motor1.getSpeed());
+    scope.set(5, setspeed);
     scope.send();
 }//let op. Geen pidposition(0,0,true) deze moet zelf zorgen dat hij 0 wordt, en daar genoeg tijd voor hebben!!!
\ No newline at end of file