set home joint 3+4
Dependencies: mbed
main.cpp@2:ee6d40dd8fb6, 2019-04-20 (annotated)
- Committer:
- pbdt1997
- Date:
- Sat Apr 20 17:03:37 2019 +0000
- Revision:
- 2:ee6d40dd8fb6
- Parent:
- 1:5ed21efc1d58
700 baht interrupt driven stepper
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pbdt1997 | 0:a3b83d874092 | 1 | #include "mbed.h" |
pbdt1997 | 0:a3b83d874092 | 2 | |
pbdt1997 | 2:ee6d40dd8fb6 | 3 | InterruptIn prox1(PA_13); |
pbdt1997 | 2:ee6d40dd8fb6 | 4 | InterruptIn prox2(PA_14); |
pbdt1997 | 1:5ed21efc1d58 | 5 | Ticker stepper; |
pbdt1997 | 1:5ed21efc1d58 | 6 | //Ticker stepper2; |
pbdt1997 | 1:5ed21efc1d58 | 7 | |
pbdt1997 | 0:a3b83d874092 | 8 | Serial pc(USBTX, USBRX); |
pbdt1997 | 0:a3b83d874092 | 9 | |
pbdt1997 | 0:a3b83d874092 | 10 | SPISlave slave(PA_7, PA_6, PA_5, PA_15); //MOSI MISO CLK CS |
pbdt1997 | 0:a3b83d874092 | 11 | |
pbdt1997 | 0:a3b83d874092 | 12 | //prox detects = 0 |
pbdt1997 | 2:ee6d40dd8fb6 | 13 | //DigitalIn prox2(P/A_14); |
pbdt1997 | 2:ee6d40dd8fb6 | 14 | //DigitalIn prox1(PA_13); |
pbdt1997 | 0:a3b83d874092 | 15 | |
pbdt1997 | 0:a3b83d874092 | 16 | //Stepper Motor 1 |
pbdt1997 | 0:a3b83d874092 | 17 | DigitalOut PUL_1(D3); |
pbdt1997 | 0:a3b83d874092 | 18 | DigitalOut DR_1(PC_0); |
pbdt1997 | 0:a3b83d874092 | 19 | |
pbdt1997 | 0:a3b83d874092 | 20 | //Stepper Motor 2 |
pbdt1997 | 0:a3b83d874092 | 21 | DigitalOut PUL_2(D4); |
pbdt1997 | 0:a3b83d874092 | 22 | DigitalOut DR_2(PC_1); |
pbdt1997 | 0:a3b83d874092 | 23 | |
pbdt1997 | 1:5ed21efc1d58 | 24 | float q3 = 0, q4 = 0; |
pbdt1997 | 1:5ed21efc1d58 | 25 | float countq3 = 0, countq4 = 0; |
pbdt1997 | 1:5ed21efc1d58 | 26 | bool moveq3 = false, moveq4 = false; |
pbdt1997 | 1:5ed21efc1d58 | 27 | bool stop = false; |
pbdt1997 | 2:ee6d40dd8fb6 | 28 | float stepq3 = 0.0, stepq4 = 0.0; |
pbdt1997 | 2:ee6d40dd8fb6 | 29 | int t = 500; |
pbdt1997 | 1:5ed21efc1d58 | 30 | |
pbdt1997 | 0:a3b83d874092 | 31 | //drive stepper motor |
pbdt1997 | 1:5ed21efc1d58 | 32 | void drvStepper1(float angle){ |
pbdt1997 | 1:5ed21efc1d58 | 33 | float step = angle - q3; |
pbdt1997 | 1:5ed21efc1d58 | 34 | step = step*44.4444444; |
pbdt1997 | 0:a3b83d874092 | 35 | if(step >= 0){ |
pbdt1997 | 1:5ed21efc1d58 | 36 | for(int i=0; i<(int)step; i++){ |
pbdt1997 | 0:a3b83d874092 | 37 | DR_1 = 0; //up |
pbdt1997 | 0:a3b83d874092 | 38 | PUL_1 = 1; |
pbdt1997 | 0:a3b83d874092 | 39 | wait_ms(1); |
pbdt1997 | 0:a3b83d874092 | 40 | PUL_1 = 0; |
pbdt1997 | 0:a3b83d874092 | 41 | wait_ms(1); |
pbdt1997 | 1:5ed21efc1d58 | 42 | countq3++; |
pbdt1997 | 0:a3b83d874092 | 43 | } |
pbdt1997 | 0:a3b83d874092 | 44 | } |
pbdt1997 | 0:a3b83d874092 | 45 | else if(step < 0){ |
pbdt1997 | 0:a3b83d874092 | 46 | step = -step; |
pbdt1997 | 1:5ed21efc1d58 | 47 | for(int i=0; i<(int)step; i++){ |
pbdt1997 | 0:a3b83d874092 | 48 | DR_1 = 1; //down |
pbdt1997 | 0:a3b83d874092 | 49 | PUL_1 = 1; |
pbdt1997 | 0:a3b83d874092 | 50 | wait_ms(1); |
pbdt1997 | 0:a3b83d874092 | 51 | PUL_1 = 0; |
pbdt1997 | 0:a3b83d874092 | 52 | wait_ms(1); |
pbdt1997 | 1:5ed21efc1d58 | 53 | countq3--; |
pbdt1997 | 0:a3b83d874092 | 54 | } |
pbdt1997 | 0:a3b83d874092 | 55 | } |
pbdt1997 | 0:a3b83d874092 | 56 | } |
pbdt1997 | 0:a3b83d874092 | 57 | |
pbdt1997 | 1:5ed21efc1d58 | 58 | void drvStepper2(float angle){ |
pbdt1997 | 1:5ed21efc1d58 | 59 | // printf("hello"); |
pbdt1997 | 1:5ed21efc1d58 | 60 | float step = angle - q4; |
pbdt1997 | 1:5ed21efc1d58 | 61 | step = step*11.1111111; |
pbdt1997 | 0:a3b83d874092 | 62 | if(step >= 0){ |
pbdt1997 | 1:5ed21efc1d58 | 63 | for(int i=0; i<(int)step; i++){ |
pbdt1997 | 0:a3b83d874092 | 64 | DR_2 = 0; //up |
pbdt1997 | 0:a3b83d874092 | 65 | PUL_2 = 1; |
pbdt1997 | 0:a3b83d874092 | 66 | wait_ms(1); |
pbdt1997 | 0:a3b83d874092 | 67 | PUL_2 = 0; |
pbdt1997 | 0:a3b83d874092 | 68 | wait_ms(1); |
pbdt1997 | 1:5ed21efc1d58 | 69 | countq4++; |
pbdt1997 | 0:a3b83d874092 | 70 | } |
pbdt1997 | 0:a3b83d874092 | 71 | } |
pbdt1997 | 0:a3b83d874092 | 72 | else if(step < 0){ |
pbdt1997 | 0:a3b83d874092 | 73 | step = -step; |
pbdt1997 | 1:5ed21efc1d58 | 74 | for(int i=0; i<(int)step; i++){ |
pbdt1997 | 0:a3b83d874092 | 75 | DR_2 = 1; //down |
pbdt1997 | 0:a3b83d874092 | 76 | PUL_2 = 1; |
pbdt1997 | 0:a3b83d874092 | 77 | wait_ms(1); |
pbdt1997 | 0:a3b83d874092 | 78 | PUL_2 = 0; |
pbdt1997 | 0:a3b83d874092 | 79 | wait_ms(1); |
pbdt1997 | 1:5ed21efc1d58 | 80 | countq4--; |
pbdt1997 | 0:a3b83d874092 | 81 | } |
pbdt1997 | 0:a3b83d874092 | 82 | } |
pbdt1997 | 0:a3b83d874092 | 83 | } |
pbdt1997 | 0:a3b83d874092 | 84 | |
pbdt1997 | 2:ee6d40dd8fb6 | 85 | void convertStep(double angle, char joint){ |
pbdt1997 | 2:ee6d40dd8fb6 | 86 | if(joint == 3){ |
pbdt1997 | 2:ee6d40dd8fb6 | 87 | stepq3 = angle - q3; |
pbdt1997 | 2:ee6d40dd8fb6 | 88 | stepq3 = stepq3*(44.0 + 4.0/9.0); |
pbdt1997 | 2:ee6d40dd8fb6 | 89 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 90 | else if(joint == 4){ |
pbdt1997 | 2:ee6d40dd8fb6 | 91 | stepq4 = angle - q4; |
pbdt1997 | 2:ee6d40dd8fb6 | 92 | stepq4 = stepq4*(1.0 + 1.0/9.0); |
pbdt1997 | 1:5ed21efc1d58 | 93 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 94 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 95 | |
pbdt1997 | 2:ee6d40dd8fb6 | 96 | void driveStepper(){ |
pbdt1997 | 2:ee6d40dd8fb6 | 97 | //if(moveq3 == true){ |
pbdt1997 | 2:ee6d40dd8fb6 | 98 | // stepq3 = stepq3 - q3; |
pbdt1997 | 2:ee6d40dd8fb6 | 99 | // stepq3 = stepq3*(44.0 + 4.0/9.0); |
pbdt1997 | 2:ee6d40dd8fb6 | 100 | // if(countq3 >= stepq3){ |
pbdt1997 | 2:ee6d40dd8fb6 | 101 | // moveq3 = false; |
pbdt1997 | 2:ee6d40dd8fb6 | 102 | // } |
pbdt1997 | 2:ee6d40dd8fb6 | 103 | // else{ |
pbdt1997 | 2:ee6d40dd8fb6 | 104 | // DR_1 = 1; |
pbdt1997 | 2:ee6d40dd8fb6 | 105 | // PUL_1 = 1; |
pbdt1997 | 2:ee6d40dd8fb6 | 106 | // countq3++; |
pbdt1997 | 2:ee6d40dd8fb6 | 107 | // } |
pbdt1997 | 2:ee6d40dd8fb6 | 108 | // } |
pbdt1997 | 2:ee6d40dd8fb6 | 109 | // else{PUL_1 = 0;} |
pbdt1997 | 1:5ed21efc1d58 | 110 | if(moveq4 == true){ |
pbdt1997 | 2:ee6d40dd8fb6 | 111 | if(countq4 >= stepq4){ |
pbdt1997 | 2:ee6d40dd8fb6 | 112 | moveq4 = false; |
pbdt1997 | 2:ee6d40dd8fb6 | 113 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 114 | else{ |
pbdt1997 | 2:ee6d40dd8fb6 | 115 | DR_2 = 1; |
pbdt1997 | 2:ee6d40dd8fb6 | 116 | PUL_2 = 1; |
pbdt1997 | 2:ee6d40dd8fb6 | 117 | countq4++; |
pbdt1997 | 2:ee6d40dd8fb6 | 118 | } |
pbdt1997 | 1:5ed21efc1d58 | 119 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 120 | else{PUL_2 = 0;} |
pbdt1997 | 1:5ed21efc1d58 | 121 | } |
pbdt1997 | 1:5ed21efc1d58 | 122 | |
pbdt1997 | 0:a3b83d874092 | 123 | void setHome(){ |
pbdt1997 | 0:a3b83d874092 | 124 | while(prox2 == 1){ |
pbdt1997 | 0:a3b83d874092 | 125 | drvStepper2(1); |
pbdt1997 | 0:a3b83d874092 | 126 | } |
pbdt1997 | 0:a3b83d874092 | 127 | wait(0.5); |
pbdt1997 | 1:5ed21efc1d58 | 128 | drvStepper2(-138.51); |
pbdt1997 | 0:a3b83d874092 | 129 | wait(0.5); |
pbdt1997 | 1:5ed21efc1d58 | 130 | countq4 = 0; |
pbdt1997 | 1:5ed21efc1d58 | 131 | q4 = 0; |
pbdt1997 | 0:a3b83d874092 | 132 | while(prox1 == 1){ |
pbdt1997 | 0:a3b83d874092 | 133 | drvStepper1(-1); |
pbdt1997 | 0:a3b83d874092 | 134 | } |
pbdt1997 | 0:a3b83d874092 | 135 | wait(0.5); |
pbdt1997 | 1:5ed21efc1d58 | 136 | drvStepper1(18); |
pbdt1997 | 0:a3b83d874092 | 137 | wait(0.5); |
pbdt1997 | 1:5ed21efc1d58 | 138 | countq3 = 0; |
pbdt1997 | 1:5ed21efc1d58 | 139 | q3 = 0; |
pbdt1997 | 0:a3b83d874092 | 140 | } |
pbdt1997 | 0:a3b83d874092 | 141 | |
pbdt1997 | 2:ee6d40dd8fb6 | 142 | void stop_q3(){ |
pbdt1997 | 2:ee6d40dd8fb6 | 143 | PUL_1 = 0; |
pbdt1997 | 2:ee6d40dd8fb6 | 144 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 145 | |
pbdt1997 | 2:ee6d40dd8fb6 | 146 | void stop_q4(){ |
pbdt1997 | 2:ee6d40dd8fb6 | 147 | PUL_2 = 0; |
pbdt1997 | 2:ee6d40dd8fb6 | 148 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 149 | |
pbdt1997 | 0:a3b83d874092 | 150 | int main() { |
pbdt1997 | 2:ee6d40dd8fb6 | 151 | stepper.attach_us(&driveStepper, t); |
pbdt1997 | 1:5ed21efc1d58 | 152 | // stepper2.attach_us(&drvStepper2, 20); |
pbdt1997 | 2:ee6d40dd8fb6 | 153 | pc.baud(250000); |
pbdt1997 | 2:ee6d40dd8fb6 | 154 | prox1.rise(&stop_q3); |
pbdt1997 | 2:ee6d40dd8fb6 | 155 | prox2.rise(&stop_q4); |
pbdt1997 | 0:a3b83d874092 | 156 | setHome(); |
pbdt1997 | 2:ee6d40dd8fb6 | 157 | wait(2); |
pbdt1997 | 2:ee6d40dd8fb6 | 158 | countq3 = 0; |
pbdt1997 | 2:ee6d40dd8fb6 | 159 | countq4 = 0; |
pbdt1997 | 2:ee6d40dd8fb6 | 160 | moveq4 = true; |
pbdt1997 | 2:ee6d40dd8fb6 | 161 | convertStep(5.0, 4); |
pbdt1997 | 2:ee6d40dd8fb6 | 162 | // q3 = 100; |
pbdt1997 | 1:5ed21efc1d58 | 163 | while(1){ |
pbdt1997 | 2:ee6d40dd8fb6 | 164 | // printf("%.2f\n",countq3); |
pbdt1997 | 2:ee6d40dd8fb6 | 165 | printf("stepq4 %.2f countq4 %.2f\n",stepq4,countq4); |
pbdt1997 | 2:ee6d40dd8fb6 | 166 | // wait(0.1); |
pbdt1997 | 2:ee6d40dd8fb6 | 167 | // moveq3 = true; |
pbdt1997 | 2:ee6d40dd8fb6 | 168 | // q3 = 10; |
pbdt1997 | 2:ee6d40dd8fb6 | 169 | } |
pbdt1997 | 2:ee6d40dd8fb6 | 170 | // setHome(); |
pbdt1997 | 2:ee6d40dd8fb6 | 171 | // printf("q3 = %.2f, q4 = %.2f\n", countq3,countq4); |
pbdt1997 | 2:ee6d40dd8fb6 | 172 | // while(1){ |
pbdt1997 | 2:ee6d40dd8fb6 | 173 | // moveq4 = true; |
pbdt1997 | 2:ee6d40dd8fb6 | 174 | // q4 = 10; |
pbdt1997 | 1:5ed21efc1d58 | 175 | // printf("done\n"); |
pbdt1997 | 1:5ed21efc1d58 | 176 | // moveq3 = false; |
pbdt1997 | 1:5ed21efc1d58 | 177 | // break; |
pbdt1997 | 2:ee6d40dd8fb6 | 178 | // wait(5); |
pbdt1997 | 2:ee6d40dd8fb6 | 179 | // } |
pbdt1997 | 1:5ed21efc1d58 | 180 | // drvStepper1(2000); |
pbdt1997 | 0:a3b83d874092 | 181 | } |