Diff: Swing.h

Revision:

0:b613dc16f27d

Child:

7:4aa57a35ec0b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Swing.h	Wed Oct 28 09:03:19 2015 +0000
@@ -0,0 +1,66 @@
+#ifndef SWING_H
+#define SWING_H
+
+/***Swing.***/
+PID contSwing(20.917 ,2.0 ,0.0 ,RATE);
+Ticker interruptSwingSpeed;
+
+int resetSwingSpeed = 0;
+
+inline void countSwingSpeed() {
+    static int i = 0;
+    if (resetSwingSpeed == 1) {
+        i = 0;
+        resetSwingSpeed = 0;
+    }
+    if (i <= swingspeed) {
+        i+=swingspeed/10.0;
+        targSwingRadVelocity=i;
+    }
+    else {
+        interruptSwingSpeed.detach(); 
+    }
+}
+
+
+inline void initializeSwing() {
+    interrupter.rise(&countSwing);
+    Motor_swing.period_us(SWING_PERIOD);
+    //controller set
+    contSwing.setInputLimits(0.0, 100.0);
+    contSwing.setOutputLimits(0.0, 1.0);
+    contSwing.setBias(0.0);
+    contSwing.setMode(AUTO_MODE);
+}
+float waitTime = 0.0;
+void countSwing() {
+    Indicator1 = !Indicator1;
+    if(enableShoot){
+        enableShoot=0;
+//        waitTime = (float)(((2.0 * PI) / swingRadVelocity) / 2.0);
+//        waitTime = (float)((((10.0 * PI) / 4.0) / swingRadVelocity) / 8.0);
+        if(swingRadVelocity==0.0) waitTime = (float)((((2.0 * PI) / swingRadVelocity) / 4.0));//-0.0017);
+        else waitTime=0.0;
+        if(waitTime<0.0) waitTime=0.0;
+        if(waitTime>10.0) waitTime=10.0;
+        wait(waitTime);
+        sendData(1,6);
+    }
+}
+
+double swingSita=0.0;
+inline void mesureSwing() {
+    PulsesSwing       = ( double )SwingSens.getPulses();
+    swingRadVelocity  = (((PulsesSwing - PrefPulsesSwing) / 3000.0) * 2.0 * PI) / RATE;
+    PrefPulsesSwing   = PulsesSwing;
+}
+
+double cont=0.0;
+inline void swingFollowing(){
+    contSwing.setSetPoint(( float )targSwingRadVelocity);
+    contSwing.setProcessValue(( float )swingRadVelocity);
+    cont = contSwing.compute();
+    Motor_swing = cont;
+}
+
+#endif /*Swing.h*/
\ No newline at end of file