Amber Tang
/
Control_JS_LimitSwitch_Lifting_20170120_ok
132
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); } }