Diff: Swing.h

Revision:

24:6d2573d6f2b6

Parent:

7:4aa57a35ec0b

Child:

40:aed70f4dcd06

--- a/Swing.h	Sat Nov 07 01:03:46 2015 +0000
+++ b/Swing.h	Sun Nov 08 04:26:26 2015 +0000
@@ -2,15 +2,15 @@
 #define SWING_H
 
 /***Swing.***/
-PID contSwing(20.917 ,2.0 ,0.0 ,RATE);
+PID contSwing(5.917 ,30.0 ,0.0 ,RATE);
 Ticker interruptSwingSpeed;
 
 int resetSwingSpeed = 0;
 
 inline void countSwingSpeed() {
-    static int i = 0;
+    static double i = 0.0;
     if (resetSwingSpeed == 1) {
-        i = 0;
+        i = 0.0;
         resetSwingSpeed = 0;
     }
     if (i <= swingspeed) {
@@ -18,37 +18,31 @@
         targSwingRadVelocity=i;
     }
     else {
-        interruptSwingSpeed.detach(); 
+        interruptSwingSpeed.detach();
     }
 }
 
 
 inline void initializeSwing() {
-    interrupter.rise(&countSwing);
     Motor_swing.period_us(SWING_PERIOD);
     //controller set
-    contSwing.setInputLimits(0.0, 100.0);
+    contSwing.setInputLimits(0.0, 50.0);
     contSwing.setOutputLimits(0.0, 1.0);
     contSwing.setBias(0.0);
     contSwing.setMode(AUTO_MODE);
 }
 
-void countSwing() {
-    Indicator1 = !Indicator1;
-    if(enableShoot){
-        enableShoot=0;
-        sendData(2,15);
-    }
-}
-
 double swingSita=0.0;
+double Rad=0.0;
 inline void mesureSwing() {
     PulsesSwing       = ( double )SwingSens.getPulses();
-    swingRadVelocity  = (((PulsesSwing - PrefPulsesSwing) / 3000.0) * 2.0 * PI) / RATE;
+    swingRadVelocity  = (((PulsesSwing - PrefPulsesSwing) / 1445.0) * 2.0 * PI) / RATE;
+    Rad = (PulsesSwing / 1445.0) * 2.0 * PI;
+    if(Rad>2*PI) Rad-=2*Rad;
     PrefPulsesSwing   = PulsesSwing;
 }
 
-double cont=0.0;
+float cont=0.0;
 inline void swingFollowing(){
     contSwing.setSetPoint(( float )targSwingRadVelocity);
     contSwing.setProcessValue(( float )swingRadVelocity);