132

Dependencies:   Servo mbed

main.cpp

Committer:
Amber77
Date:
2017-09-20
Revision:
0:84c1f6c3e4ff

File content as of revision 0:84c1f6c3e4ff:

/* 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);
        }    
}