Amber Tang / Mbed 2 deprecated Control_JS_LimitSwitch_Lifting_20170120_ok

132

Dependencies:   Servo mbed

main.cpp@0:84c1f6c3e4ff, 2017-09-20 (annotated)

Committer:
Amber77
Date:
Wed Sep 20 20:37:27 2017 +0000
Revision:
0:84c1f6c3e4ff
20170921

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Amber77 0:84c1f6c3e4ff 1 /* https://developer.mbed.org/users/andrewrussell/code/Servo/ */
Amber77 0:84c1f6c3e4ff 2 /* JS using 3.3V */
Amber77 0:84c1f6c3e4ff 3 #include "mbed.h"
Amber77 0:84c1f6c3e4ff 4 #include "Servo.h"
Amber77 0:84c1f6c3e4ff 5
Amber77 0:84c1f6c3e4ff 6 //DigitalOut StopRun(D11);
Amber77 0:84c1f6c3e4ff 7 //DigitalOut F_R(D10); // CW/CCW
Amber77 0:84c1f6c3e4ff 8 //DigitalOut H_F(D9);
Amber77 0:84c1f6c3e4ff 9 //DigitalOut StopRun(PA_11);
Amber77 0:84c1f6c3e4ff 10 //DigitalOut F_R(PB_2); // CW/CCW
Amber77 0:84c1f6c3e4ff 11 //DigitalOut H_F(PB_1);
Amber77 0:84c1f6c3e4ff 12 DigitalOut StopRun(PA_15);
Amber77 0:84c1f6c3e4ff 13 DigitalOut F_R(PA_14); // CW/CCW
Amber77 0:84c1f6c3e4ff 14 DigitalOut H_F(PA_13);
Amber77 0:84c1f6c3e4ff 15 //DigitalOut StopRun(PB_13);
Amber77 0:84c1f6c3e4ff 16 //DigitalOut F_R(PB_14); // CW/CCW
Amber77 0:84c1f6c3e4ff 17 //DigitalOut H_F(PB_15);
Amber77 0:84c1f6c3e4ff 18 AnalogIn vx(A2);
Amber77 0:84c1f6c3e4ff 19 AnalogIn vy(A3);
Amber77 0:84c1f6c3e4ff 20 AnalogIn ps(A4);
Amber77 0:84c1f6c3e4ff 21
Amber77 0:84c1f6c3e4ff 22 //DigitalIn LimitSwitchUp(D6,PullUp);
Amber77 0:84c1f6c3e4ff 23 //DigitalIn LimitSwitchDown(D3,PullUp);
Amber77 0:84c1f6c3e4ff 24 DigitalIn LimitSwitchUp(PB_15,PullUp);
Amber77 0:84c1f6c3e4ff 25 DigitalIn LimitSwitchDown(PB_14,PullUp);
Amber77 0:84c1f6c3e4ff 26 //DigitalIn TIM(D6);
Amber77 0:84c1f6c3e4ff 27
Amber77 0:84c1f6c3e4ff 28 DigitalOut myled(LED1);
Amber77 0:84c1f6c3e4ff 29
Amber77 0:84c1f6c3e4ff 30 int main()
Amber77 0:84c1f6c3e4ff 31 {
Amber77 0:84c1f6c3e4ff 32 float x, y = 0;
Amber77 0:84c1f6c3e4ff 33 float cx, cy = 0;
Amber77 0:84c1f6c3e4ff 34 int px, py = 0;
Amber77 0:84c1f6c3e4ff 35 int bia = 3;
Amber77 0:84c1f6c3e4ff 36 float pos = 0;
Amber77 0:84c1f6c3e4ff 37 int limitSwitchUp;
Amber77 0:84c1f6c3e4ff 38 int limitSwitchDown;
Amber77 0:84c1f6c3e4ff 39 //TIM.calibrate(0.00001, 0.08*0.00001, 1*0.00001); //10kHZ
Amber77 0:84c1f6c3e4ff 40 cx = vx.read();
Amber77 0:84c1f6c3e4ff 41 cy = vy.read();
Amber77 0:84c1f6c3e4ff 42 H_F.write(1);
Amber77 0:84c1f6c3e4ff 43 while(1)
Amber77 0:84c1f6c3e4ff 44 {
Amber77 0:84c1f6c3e4ff 45 x = vx.read();
Amber77 0:84c1f6c3e4ff 46 y = vy.read();
Amber77 0:84c1f6c3e4ff 47 limitSwitchUp = LimitSwitchUp.read();
Amber77 0:84c1f6c3e4ff 48 limitSwitchDown = LimitSwitchDown.read();
Amber77 0:84c1f6c3e4ff 49 /* x-axis re-arrange */
Amber77 0:84c1f6c3e4ff 50 if(x > cx)
Amber77 0:84c1f6c3e4ff 51 {
Amber77 0:84c1f6c3e4ff 52 px = (x-cx)/(1-cx)*100;
Amber77 0:84c1f6c3e4ff 53 if(px < bia)
Amber77 0:84c1f6c3e4ff 54 {
Amber77 0:84c1f6c3e4ff 55 px = 0;
Amber77 0:84c1f6c3e4ff 56 StopRun.write(1);
Amber77 0:84c1f6c3e4ff 57 F_R.write(1); // counterclockwise:0 clockwise:1
Amber77 0:84c1f6c3e4ff 58 pos = 0;
Amber77 0:84c1f6c3e4ff 59 }
Amber77 0:84c1f6c3e4ff 60 else
Amber77 0:84c1f6c3e4ff 61 {
Amber77 0:84c1f6c3e4ff 62 if(limitSwitchUp == 1)
Amber77 0:84c1f6c3e4ff 63 {
Amber77 0:84c1f6c3e4ff 64 StopRun.write(0); // RUN ---turn up
Amber77 0:84c1f6c3e4ff 65 F_R.write(1); // counterclockwise:0 clockwise:1
Amber77 0:84c1f6c3e4ff 66 pos=px*0.01;
Amber77 0:84c1f6c3e4ff 67 }
Amber77 0:84c1f6c3e4ff 68 else
Amber77 0:84c1f6c3e4ff 69 {
Amber77 0:84c1f6c3e4ff 70 StopRun.write(1); // STOP
Amber77 0:84c1f6c3e4ff 71 F_R.write(1);
Amber77 0:84c1f6c3e4ff 72 }
Amber77 0:84c1f6c3e4ff 73 }
Amber77 0:84c1f6c3e4ff 74 }
Amber77 0:84c1f6c3e4ff 75 else if(x < cx)
Amber77 0:84c1f6c3e4ff 76 {
Amber77 0:84c1f6c3e4ff 77 px = (x-cx)/(cx-0)*100;
Amber77 0:84c1f6c3e4ff 78 if(px > -bia)
Amber77 0:84c1f6c3e4ff 79 {
Amber77 0:84c1f6c3e4ff 80 px = 0;
Amber77 0:84c1f6c3e4ff 81 StopRun.write(1);
Amber77 0:84c1f6c3e4ff 82 F_R.write(0); // counterclockwise:0 clockwise:1
Amber77 0:84c1f6c3e4ff 83 pos = 0;
Amber77 0:84c1f6c3e4ff 84 }
Amber77 0:84c1f6c3e4ff 85 else
Amber77 0:84c1f6c3e4ff 86 {
Amber77 0:84c1f6c3e4ff 87 if(limitSwitchDown == 1)
Amber77 0:84c1f6c3e4ff 88 {
Amber77 0:84c1f6c3e4ff 89 StopRun.write(0); // RUN ---turn down
Amber77 0:84c1f6c3e4ff 90 F_R.write(0); // counterclockwise:0 clockwise:1
Amber77 0:84c1f6c3e4ff 91 pos=px*(-0.01);
Amber77 0:84c1f6c3e4ff 92 }
Amber77 0:84c1f6c3e4ff 93 else
Amber77 0:84c1f6c3e4ff 94 {
Amber77 0:84c1f6c3e4ff 95 StopRun.write(1); // STOP
Amber77 0:84c1f6c3e4ff 96 F_R.write(0);
Amber77 0:84c1f6c3e4ff 97 }
Amber77 0:84c1f6c3e4ff 98
Amber77 0:84c1f6c3e4ff 99 }
Amber77 0:84c1f6c3e4ff 100 }
Amber77 0:84c1f6c3e4ff 101 else
Amber77 0:84c1f6c3e4ff 102 px=cx;
Amber77 0:84c1f6c3e4ff 103 /* y-axis re-arrange */
Amber77 0:84c1f6c3e4ff 104 if(y > cy)
Amber77 0:84c1f6c3e4ff 105 {
Amber77 0:84c1f6c3e4ff 106 py = (y-cy)/(1-cy)*100;
Amber77 0:84c1f6c3e4ff 107 if(py < bia)
Amber77 0:84c1f6c3e4ff 108 py = 0;
Amber77 0:84c1f6c3e4ff 109 }
Amber77 0:84c1f6c3e4ff 110 else if(y < cy)
Amber77 0:84c1f6c3e4ff 111 {
Amber77 0:84c1f6c3e4ff 112 py = (y-cy)/(cy-0)*100;
Amber77 0:84c1f6c3e4ff 113 if(py > -bia)
Amber77 0:84c1f6c3e4ff 114 py = 0;
Amber77 0:84c1f6c3e4ff 115 }
Amber77 0:84c1f6c3e4ff 116 else
Amber77 0:84c1f6c3e4ff 117 py=cy;
Amber77 0:84c1f6c3e4ff 118
Amber77 0:84c1f6c3e4ff 119 //TIM.write(pos);
Amber77 0:84c1f6c3e4ff 120
Amber77 0:84c1f6c3e4ff 121 printf("%f, %d, %f\n",x,px,pos);
Amber77 0:84c1f6c3e4ff 122 printf(" %d | %d\n", limitSwitchUp,limitSwitchDown);
Amber77 0:84c1f6c3e4ff 123
Amber77 0:84c1f6c3e4ff 124 /*
Amber77 0:84c1f6c3e4ff 125 press = ps.read();
Amber77 0:84c1f6c3e4ff 126 if(press < 0.008)
Amber77 0:84c1f6c3e4ff 127 printf("Pressed: %d, %d\n",px,py);
Amber77 0:84c1f6c3e4ff 128 else
Amber77 0:84c1f6c3e4ff 129 printf(" : %d, %d\n",px,py);
Amber77 0:84c1f6c3e4ff 130 */
Amber77 0:84c1f6c3e4ff 131 wait_ms(0.1);
Amber77 0:84c1f6c3e4ff 132 }
Amber77 0:84c1f6c3e4ff 133 }