set home joint 3+4
Dependencies: mbed
main.cpp@5:d86a0c29dd29, 2019-04-21 (annotated)
- Committer:
- pbdt1997
- Date:
- Sun Apr 21 14:06:22 2019 +0000
- Revision:
- 5:d86a0c29dd29
- Parent:
- 4:fb0905390ebc
just pray
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 | 4:fb0905390ebc | 3 | InterruptIn prox1(PA_13); |
pbdt1997 | 4:fb0905390ebc | 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 | //Stepper Motor 1 |
pbdt1997 | 0:a3b83d874092 | 13 | DigitalOut PUL_1(D3); |
pbdt1997 | 0:a3b83d874092 | 14 | DigitalOut DR_1(PC_0); |
pbdt1997 | 0:a3b83d874092 | 15 | |
pbdt1997 | 0:a3b83d874092 | 16 | //Stepper Motor 2 |
pbdt1997 | 0:a3b83d874092 | 17 | DigitalOut PUL_2(D4); |
pbdt1997 | 0:a3b83d874092 | 18 | DigitalOut DR_2(PC_1); |
pbdt1997 | 0:a3b83d874092 | 19 | |
pbdt1997 | 5:d86a0c29dd29 | 20 | |
pbdt1997 | 5:d86a0c29dd29 | 21 | //globals |
pbdt1997 | 5:d86a0c29dd29 | 22 | double q3 = 0, q4 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 23 | double q3_count = 0, q4_count = 0; |
pbdt1997 | 5:d86a0c29dd29 | 24 | double q3_speed = 1, q4_speed = 1; |
pbdt1997 | 5:d86a0c29dd29 | 25 | double q3_step = 0, q4_step = 0; |
pbdt1997 | 1:5ed21efc1d58 | 26 | bool moveq3 = false, moveq4 = false; |
pbdt1997 | 1:5ed21efc1d58 | 27 | bool stop = false; |
pbdt1997 | 5:d86a0c29dd29 | 28 | double t = 10000.0; |
pbdt1997 | 5:d86a0c29dd29 | 29 | |
pbdt1997 | 5:d86a0c29dd29 | 30 | //Functions |
pbdt1997 | 1:5ed21efc1d58 | 31 | |
pbdt1997 | 5:d86a0c29dd29 | 32 | //conversion funcs |
pbdt1997 | 5:d86a0c29dd29 | 33 | void convertStep(double angle, char joint){ |
pbdt1997 | 5:d86a0c29dd29 | 34 | if(joint == 3){ |
pbdt1997 | 5:d86a0c29dd29 | 35 | q3_step = angle - q3; |
pbdt1997 | 5:d86a0c29dd29 | 36 | q3_step = abs(q3_step*(44.444444)); |
pbdt1997 | 0:a3b83d874092 | 37 | } |
pbdt1997 | 5:d86a0c29dd29 | 38 | else if(joint == 4){ |
pbdt1997 | 5:d86a0c29dd29 | 39 | q4_step = angle - q4; |
pbdt1997 | 5:d86a0c29dd29 | 40 | // q4_step = q4_step*(10.0 + 1.0/9.0); |
pbdt1997 | 5:d86a0c29dd29 | 41 | q4_step = abs(q4_step*(11.1111111)); |
pbdt1997 | 0:a3b83d874092 | 42 | } |
pbdt1997 | 0:a3b83d874092 | 43 | } |
pbdt1997 | 0:a3b83d874092 | 44 | |
pbdt1997 | 0:a3b83d874092 | 45 | |
pbdt1997 | 5:d86a0c29dd29 | 46 | //ISRs// |
pbdt1997 | 4:fb0905390ebc | 47 | |
pbdt1997 | 5:d86a0c29dd29 | 48 | //Proximity sensors |
pbdt1997 | 4:fb0905390ebc | 49 | void stop_q3(){ |
pbdt1997 | 4:fb0905390ebc | 50 | PUL_1 = 0; |
pbdt1997 | 4:fb0905390ebc | 51 | } |
pbdt1997 | 4:fb0905390ebc | 52 | |
pbdt1997 | 4:fb0905390ebc | 53 | void stop_q4(){ |
pbdt1997 | 4:fb0905390ebc | 54 | PUL_2 = 0; |
pbdt1997 | 4:fb0905390ebc | 55 | } |
pbdt1997 | 4:fb0905390ebc | 56 | |
pbdt1997 | 5:d86a0c29dd29 | 57 | // |
pbdt1997 | 5:d86a0c29dd29 | 58 | void driveStepper(){ |
pbdt1997 | 5:d86a0c29dd29 | 59 | if(moveq4 == true){ |
pbdt1997 | 5:d86a0c29dd29 | 60 | if(q4_count >= q4_step){ |
pbdt1997 | 5:d86a0c29dd29 | 61 | moveq4 = false; |
pbdt1997 | 5:d86a0c29dd29 | 62 | if(DR_2 == 0){ |
pbdt1997 | 5:d86a0c29dd29 | 63 | q4 += q4_count/11.1111111; |
pbdt1997 | 5:d86a0c29dd29 | 64 | }else{ |
pbdt1997 | 5:d86a0c29dd29 | 65 | q4 -= q4_count/11.1111111; |
pbdt1997 | 5:d86a0c29dd29 | 66 | } |
pbdt1997 | 5:d86a0c29dd29 | 67 | q4_count = 0; |
pbdt1997 | 5:d86a0c29dd29 | 68 | PUL_2 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 69 | |
pbdt1997 | 5:d86a0c29dd29 | 70 | } |
pbdt1997 | 5:d86a0c29dd29 | 71 | else{ |
pbdt1997 | 5:d86a0c29dd29 | 72 | if((uint8_t)q4_count%(uint8_t)q4_speed==0){ |
pbdt1997 | 5:d86a0c29dd29 | 73 | PUL_2 = !PUL_2; |
pbdt1997 | 5:d86a0c29dd29 | 74 | } |
pbdt1997 | 5:d86a0c29dd29 | 75 | q4_count++; |
pbdt1997 | 5:d86a0c29dd29 | 76 | |
pbdt1997 | 5:d86a0c29dd29 | 77 | } |
pbdt1997 | 5:d86a0c29dd29 | 78 | } |
pbdt1997 | 5:d86a0c29dd29 | 79 | if(moveq3 == true){ |
pbdt1997 | 5:d86a0c29dd29 | 80 | if(q3_count >= q3_step){ |
pbdt1997 | 5:d86a0c29dd29 | 81 | moveq3 = false; |
pbdt1997 | 5:d86a0c29dd29 | 82 | if(DR_1 == 0){ |
pbdt1997 | 5:d86a0c29dd29 | 83 | q3 += q3_count/44.4444444; |
pbdt1997 | 5:d86a0c29dd29 | 84 | }else{ |
pbdt1997 | 5:d86a0c29dd29 | 85 | q3 -= q3_count/44.4444444; |
pbdt1997 | 5:d86a0c29dd29 | 86 | } |
pbdt1997 | 5:d86a0c29dd29 | 87 | q3_count = 0; |
pbdt1997 | 5:d86a0c29dd29 | 88 | PUL_1 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 89 | } |
pbdt1997 | 5:d86a0c29dd29 | 90 | else{ |
pbdt1997 | 5:d86a0c29dd29 | 91 | if((uint8_t)q3_count%(uint8_t)q3_speed==0){ |
pbdt1997 | 5:d86a0c29dd29 | 92 | PUL_1 = !PUL_1; |
pbdt1997 | 5:d86a0c29dd29 | 93 | } |
pbdt1997 | 5:d86a0c29dd29 | 94 | q3_count++; |
pbdt1997 | 5:d86a0c29dd29 | 95 | |
pbdt1997 | 5:d86a0c29dd29 | 96 | } |
pbdt1997 | 5:d86a0c29dd29 | 97 | } |
pbdt1997 | 5:d86a0c29dd29 | 98 | } |
pbdt1997 | 5:d86a0c29dd29 | 99 | |
pbdt1997 | 5:d86a0c29dd29 | 100 | void drvStepper1(double angle, double speed){ |
pbdt1997 | 5:d86a0c29dd29 | 101 | q3_speed = speed; |
pbdt1997 | 5:d86a0c29dd29 | 102 | if(angle - q3 >= 0){ |
pbdt1997 | 5:d86a0c29dd29 | 103 | DR_1 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 104 | } |
pbdt1997 | 5:d86a0c29dd29 | 105 | else{ |
pbdt1997 | 5:d86a0c29dd29 | 106 | DR_1 = 1; |
pbdt1997 | 5:d86a0c29dd29 | 107 | } |
pbdt1997 | 5:d86a0c29dd29 | 108 | moveq3 = true; |
pbdt1997 | 5:d86a0c29dd29 | 109 | convertStep(angle, 3); |
pbdt1997 | 5:d86a0c29dd29 | 110 | } |
pbdt1997 | 5:d86a0c29dd29 | 111 | |
pbdt1997 | 5:d86a0c29dd29 | 112 | void drvStepper2(double angle, double speed){ |
pbdt1997 | 5:d86a0c29dd29 | 113 | // stepper.detach(); |
pbdt1997 | 5:d86a0c29dd29 | 114 | // stepper.attach_us(&driveStepper, 1000000/(speed)); |
pbdt1997 | 5:d86a0c29dd29 | 115 | q4_speed = speed; |
pbdt1997 | 4:fb0905390ebc | 116 | if(angle - q4 >= 0){ |
pbdt1997 | 4:fb0905390ebc | 117 | DR_2 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 118 | } |
pbdt1997 | 4:fb0905390ebc | 119 | else{ |
pbdt1997 | 4:fb0905390ebc | 120 | DR_2 = 1; |
pbdt1997 | 5:d86a0c29dd29 | 121 | } |
pbdt1997 | 1:5ed21efc1d58 | 122 | moveq4 = true; |
pbdt1997 | 4:fb0905390ebc | 123 | convertStep(angle, 4); |
pbdt1997 | 5:d86a0c29dd29 | 124 | } |
pbdt1997 | 5:d86a0c29dd29 | 125 | |
pbdt1997 | 5:d86a0c29dd29 | 126 | void veloControl(double q3_trgt, double q4_trgt, double v3, double v4){ |
pbdt1997 | 5:d86a0c29dd29 | 127 | //wait till previous movement stopped |
pbdt1997 | 5:d86a0c29dd29 | 128 | while(moveq4||moveq3){ |
pbdt1997 | 5:d86a0c29dd29 | 129 | } |
pbdt1997 | 5:d86a0c29dd29 | 130 | drvStepper1(q3_trgt, v3); |
pbdt1997 | 5:d86a0c29dd29 | 131 | drvStepper2(q4_trgt, v4); |
pbdt1997 | 5:d86a0c29dd29 | 132 | moveq3 = true; |
pbdt1997 | 5:d86a0c29dd29 | 133 | moveq4 = true; |
pbdt1997 | 5:d86a0c29dd29 | 134 | } |
pbdt1997 | 5:d86a0c29dd29 | 135 | |
pbdt1997 | 5:d86a0c29dd29 | 136 | void setHome(){ |
pbdt1997 | 5:d86a0c29dd29 | 137 | //while(prox2 == 1){ |
pbdt1997 | 5:d86a0c29dd29 | 138 | // drvStepper2(1, 1000.0); |
pbdt1997 | 5:d86a0c29dd29 | 139 | // } |
pbdt1997 | 5:d86a0c29dd29 | 140 | // wait(0.5); |
pbdt1997 | 5:d86a0c29dd29 | 141 | // drvStepper2(-138.51); |
pbdt1997 | 5:d86a0c29dd29 | 142 | // wait(0.5); |
pbdt1997 | 5:d86a0c29dd29 | 143 | // q4 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 144 | // q4 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 145 | // while(prox1 == 1){ |
pbdt1997 | 5:d86a0c29dd29 | 146 | // drvStepper1(-1); |
pbdt1997 | 5:d86a0c29dd29 | 147 | // } |
pbdt1997 | 5:d86a0c29dd29 | 148 | // wait(0.5); |
pbdt1997 | 5:d86a0c29dd29 | 149 | // drvStepper1(18); |
pbdt1997 | 5:d86a0c29dd29 | 150 | // wait(0.5); |
pbdt1997 | 5:d86a0c29dd29 | 151 | // q3 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 152 | // q3 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 153 | } |
pbdt1997 | 5:d86a0c29dd29 | 154 | |
pbdt1997 | 5:d86a0c29dd29 | 155 | |
pbdt1997 | 5:d86a0c29dd29 | 156 | |
pbdt1997 | 4:fb0905390ebc | 157 | |
pbdt1997 | 4:fb0905390ebc | 158 | |
pbdt1997 | 4:fb0905390ebc | 159 | int main() { |
pbdt1997 | 5:d86a0c29dd29 | 160 | stepper.attach_us(&driveStepper, 100.0); |
pbdt1997 | 4:fb0905390ebc | 161 | pc.baud(250000); |
pbdt1997 | 4:fb0905390ebc | 162 | //prox1.rise(&stop_q3); |
pbdt1997 | 4:fb0905390ebc | 163 | // prox2.rise(&stop_q4); |
pbdt1997 | 4:fb0905390ebc | 164 | moveq4 = false; |
pbdt1997 | 5:d86a0c29dd29 | 165 | moveq3 = false; |
pbdt1997 | 5:d86a0c29dd29 | 166 | // setHome(); |
pbdt1997 | 5:d86a0c29dd29 | 167 | // wait(2); |
pbdt1997 | 5:d86a0c29dd29 | 168 | q3 = 0; |
pbdt1997 | 5:d86a0c29dd29 | 169 | q4 = 0; |
pbdt1997 | 4:fb0905390ebc | 170 | |
pbdt1997 | 5:d86a0c29dd29 | 171 | veloControl(10,10,10,10); |
pbdt1997 | 5:d86a0c29dd29 | 172 | printf("q3 = %.2f q4 = %.2f\n", q3, q4); |
pbdt1997 | 5:d86a0c29dd29 | 173 | while(moveq4||moveq3){ |
pbdt1997 | 5:d86a0c29dd29 | 174 | printf("numba 1 q3 %.2f q4 %.2f\n",q3_count,q4_count); |
pbdt1997 | 5:d86a0c29dd29 | 175 | } |
pbdt1997 | 5:d86a0c29dd29 | 176 | printf("q3 = %.2f q4 = %.2f\n", q3, q4); |
pbdt1997 | 5:d86a0c29dd29 | 177 | veloControl(-10,-15,10,10); |
pbdt1997 | 4:fb0905390ebc | 178 | |
pbdt1997 | 5:d86a0c29dd29 | 179 | while(moveq4||moveq3){ |
pbdt1997 | 5:d86a0c29dd29 | 180 | printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count); |
pbdt1997 | 5:d86a0c29dd29 | 181 | } |
pbdt1997 | 5:d86a0c29dd29 | 182 | printf("q3 = %.2f q4 = %.2f\n", q3, q4); |
pbdt1997 | 5:d86a0c29dd29 | 183 | // while(1){ |
pbdt1997 | 5:d86a0c29dd29 | 184 | //// drvStepper2(60.0, 1000); |
pbdt1997 | 5:d86a0c29dd29 | 185 | //// wait(5); |
pbdt1997 | 5:d86a0c29dd29 | 186 | //// drvStepper2(30.0, 1000); |
pbdt1997 | 5:d86a0c29dd29 | 187 | // printf("q4_count %.2f q4 %.2f\n",q4_count,q4); |
pbdt1997 | 5:d86a0c29dd29 | 188 | // wait_ms(10); |
pbdt1997 | 5:d86a0c29dd29 | 189 | // |
pbdt1997 | 5:d86a0c29dd29 | 190 | // } |
pbdt1997 | 4:fb0905390ebc | 191 | |
pbdt1997 | 0:a3b83d874092 | 192 | } |