Diff: main.cpp

Revision:

0:84c1f6c3e4ff

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 20 20:37:27 2017 +0000
@@ -0,0 +1,133 @@
+/* https://developer.mbed.org/users/andrewrussell/code/Servo/ */
+/* JS using 3.3V */
+#include "mbed.h"
+#include "Servo.h"
+
+//DigitalOut StopRun(D11);
+//DigitalOut F_R(D10);  // CW/CCW
+//DigitalOut H_F(D9);  
+//DigitalOut StopRun(PA_11);
+//DigitalOut F_R(PB_2);  // CW/CCW
+//DigitalOut H_F(PB_1);  
+DigitalOut StopRun(PA_15);
+DigitalOut F_R(PA_14);  // CW/CCW
+DigitalOut H_F(PA_13);
+//DigitalOut StopRun(PB_13);
+//DigitalOut F_R(PB_14);  // CW/CCW
+//DigitalOut H_F(PB_15);
+AnalogIn vx(A2);
+AnalogIn vy(A3);
+AnalogIn ps(A4);
+
+//DigitalIn LimitSwitchUp(D6,PullUp);
+//DigitalIn LimitSwitchDown(D3,PullUp);
+DigitalIn LimitSwitchUp(PB_15,PullUp);
+DigitalIn LimitSwitchDown(PB_14,PullUp);
+//DigitalIn TIM(D6);
+
+DigitalOut myled(LED1);
+
+int main() 
+{
+    float x, y = 0;
+    float cx, cy = 0;
+    int px, py = 0;
+    int bia = 3;
+    float pos = 0; 
+    int limitSwitchUp;
+    int limitSwitchDown;
+    //TIM.calibrate(0.00001, 0.08*0.00001, 1*0.00001);  //10kHZ
+    cx = vx.read();
+    cy = vy.read();
+    H_F.write(1);
+    while(1)
+    { 
+        x = vx.read();
+        y = vy.read();
+        limitSwitchUp = LimitSwitchUp.read();
+        limitSwitchDown = LimitSwitchDown.read();
+        /* x-axis re-arrange */
+        if(x > cx)
+        {
+            px = (x-cx)/(1-cx)*100;
+            if(px < bia)
+            {
+                px = 0;
+                StopRun.write(1);
+                F_R.write(1);               // counterclockwise:0    clockwise:1
+                pos = 0;
+            }
+            else
+            {
+                 if(limitSwitchUp == 1)
+                {
+                    StopRun.write(0);       // RUN ---turn up
+                    F_R.write(1);           // counterclockwise:0    clockwise:1
+                    pos=px*0.01;
+                }
+                else
+                {
+                    StopRun.write(1);       // STOP
+                    F_R.write(1);
+                }
+            }      
+        }
+        else if(x < cx)
+        {
+            px = (x-cx)/(cx-0)*100;
+            if(px > -bia)
+            {
+                px = 0;
+                StopRun.write(1);
+                F_R.write(0);               // counterclockwise:0    clockwise:1
+                pos = 0;
+            }
+            else
+            {
+                if(limitSwitchDown == 1)
+                {
+                    StopRun.write(0);       // RUN ---turn down
+                    F_R.write(0);           // counterclockwise:0    clockwise:1
+                    pos=px*(-0.01);
+                }
+                else
+                {
+                    StopRun.write(1);       // STOP
+                    F_R.write(0);
+                }
+                    
+            } 
+        }
+        else
+            px=cx;
+        /* y-axis re-arrange */
+        if(y > cy)
+        {
+            py = (y-cy)/(1-cy)*100;
+            if(py < bia)
+                py = 0;
+        }
+        else if(y < cy)
+        {
+            py = (y-cy)/(cy-0)*100;
+            if(py > -bia)
+                py = 0;
+        }
+        else
+            py=cy;
+            
+        //TIM.write(pos); 
+        
+        printf("%f, %d, %f\n",x,px,pos);
+        printf(" %d | %d\n", limitSwitchUp,limitSwitchDown);
+        
+        /*
+        press = ps.read();
+        if(press < 0.008)
+            printf("Pressed: %d, %d\n",px,py);
+        else
+            printf("       : %d, %d\n",px,py);
+        */
+        wait_ms(0.1);
+        }    
+}
\ No newline at end of file