Amber Tang
/
Control_JS_LimitSwitch_Lifting_20170120_ok
132
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?
User | Revision | Line number | New 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 | } |