Robotics Studio / Mbed 2 deprecated steppermotor_almostdone

set home joint 3+4

Dependencies:   mbed

main.cpp@7:8d69b4230a02, 2019-04-23 (annotated)

Committer:
choyai
Date:
Tue Apr 23 07:17:41 2019 +0000
Revision:
7:8d69b4230a02
Parent:
6:ce7c403ad32a 
untested spi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbdt1997 0:a3b83d874092 1 #include "mbed.h"
pbdt1997 0:a3b83d874092 2
pbdt1997 6:ce7c403ad32a 3 InterruptIn prox1(PA_14);
pbdt1997 6:ce7c403ad32a 4 InterruptIn prox2(PA_13);
choyai 7:8d69b4230a02 5 // DigitalIn prox2(PA_14);
pbdt1997 1:5ed21efc1d58 6 Ticker stepper;
choyai 7:8d69b4230a02 7 // Ticker stepper2;
choyai 7:8d69b4230a02 8 // DigitalOut led(LED1); //on-board led
pbdt1997 0:a3b83d874092 9 Serial pc(USBTX, USBRX);
pbdt1997 0:a3b83d874092 10
choyai 7:8d69b4230a02 11 SPISlave slave(PA_7, PA_6, PA_5, PA_15); // MOSI MISO CLK CS
pbdt1997 0:a3b83d874092 12
choyai 7:8d69b4230a02 13 // Stepper Motor 1
pbdt1997 0:a3b83d874092 14 DigitalOut PUL_1(D3);
pbdt1997 0:a3b83d874092 15 DigitalOut DR_1(PC_0);
pbdt1997 0:a3b83d874092 16
choyai 7:8d69b4230a02 17 // Stepper Motor 2
pbdt1997 0:a3b83d874092 18 DigitalOut PUL_2(D4);
pbdt1997 0:a3b83d874092 19 DigitalOut DR_2(PC_1);
pbdt1997 0:a3b83d874092 20
choyai 7:8d69b4230a02 21 // globals
choyai 7:8d69b4230a02 22 char array[20];
choyai 7:8d69b4230a02 23 char SPIarr[20];
choyai 7:8d69b4230a02 24 int SPIgot = 0;
choyai 7:8d69b4230a02 25 char spiComID;
choyai 7:8d69b4230a02 26 char SM_id = 0, data_id = 0;
choyai 7:8d69b4230a02 27 int getPackage = 0;
choyai 7:8d69b4230a02 28 char command_ID;
choyai 7:8d69b4230a02 29 char arraySize = sizeof(array);
choyai 7:8d69b4230a02 30 char getData = 0;
choyai 7:8d69b4230a02 31 char checkData = 0;
pbdt1997 5:d86a0c29dd29 32 double q3 = 0, q4 = 0;
pbdt1997 5:d86a0c29dd29 33 double q3_count = 0, q4_count = 0;
pbdt1997 5:d86a0c29dd29 34 double q3_speed = 1, q4_speed = 1;
pbdt1997 5:d86a0c29dd29 35 double q3_step = 0, q4_step = 0;
pbdt1997 1:5ed21efc1d58 36 bool moveq3 = false, moveq4 = false;
pbdt1997 1:5ed21efc1d58 37 bool stop = false;
pbdt1997 6:ce7c403ad32a 38 bool prox1_flag = false;
pbdt1997 6:ce7c403ad32a 39 bool prox2_flag = false;
pbdt1997 5:d86a0c29dd29 40 double t = 10000.0;
pbdt1997 5:d86a0c29dd29 41
choyai 7:8d69b4230a02 42 // Functions
pbdt1997 1:5ed21efc1d58 43
choyai 7:8d69b4230a02 44 // conversion funcs
choyai 7:8d69b4230a02 45 void convertStep(double angle, char joint) {
choyai 7:8d69b4230a02 46 if (joint == 3) {
choyai 7:8d69b4230a02 47 q3_step = angle - q3;
choyai 7:8d69b4230a02 48 q3_step = abs(q3_step * (22.2222222));
choyai 7:8d69b4230a02 49 } else if (joint == 4) {
choyai 7:8d69b4230a02 50 q4_step = angle - q4;
choyai 7:8d69b4230a02 51 // q4_step = q4_step*(10.0 + 1.0/9.0);
choyai 7:8d69b4230a02 52 q4_step = abs(q4_step * (11.1111111));
choyai 7:8d69b4230a02 53 }
pbdt1997 0:a3b83d874092 54 }
pbdt1997 0:a3b83d874092 55
choyai 7:8d69b4230a02 56 // ISRs//
pbdt1997 4:fb0905390ebc 57
choyai 7:8d69b4230a02 58 // Proximity sensors
choyai 7:8d69b4230a02 59 void stop_q3() {
choyai 7:8d69b4230a02 60 PUL_1 = 0;
choyai 7:8d69b4230a02 61 prox1_flag = true;
choyai 7:8d69b4230a02 62 moveq3 = false;
choyai 7:8d69b4230a02 63 // led = !led;
pbdt1997 4:fb0905390ebc 64 }
pbdt1997 4:fb0905390ebc 65
choyai 7:8d69b4230a02 66 void stop_q4() {
choyai 7:8d69b4230a02 67 PUL_2 = 0;
choyai 7:8d69b4230a02 68 prox2_flag = true;
choyai 7:8d69b4230a02 69 moveq4 = false;
pbdt1997 4:fb0905390ebc 70 }
pbdt1997 4:fb0905390ebc 71
pbdt1997 5:d86a0c29dd29 72 //
choyai 7:8d69b4230a02 73 void driveStepper() {
choyai 7:8d69b4230a02 74 if (moveq4 == true) {
choyai 7:8d69b4230a02 75 if (q4_count >= q4_step) {
choyai 7:8d69b4230a02 76 moveq4 = false;
choyai 7:8d69b4230a02 77 if (DR_2 == 0) {
choyai 7:8d69b4230a02 78 q4 += q4_count / 11.1111111;
choyai 7:8d69b4230a02 79 } else {
choyai 7:8d69b4230a02 80 q4 -= q4_count / 11.1111111;
choyai 7:8d69b4230a02 81 }
choyai 7:8d69b4230a02 82 q4_count = 0;
choyai 7:8d69b4230a02 83 PUL_2 = 0;
pbdt1997 5:d86a0c29dd29 84
choyai 7:8d69b4230a02 85 } else {
choyai 7:8d69b4230a02 86 if ((uint8_t)q4_count % (uint8_t)q4_speed == 0) {
choyai 7:8d69b4230a02 87 PUL_2 = !PUL_2;
choyai 7:8d69b4230a02 88 q4_count++;
choyai 7:8d69b4230a02 89 }
choyai 7:8d69b4230a02 90 }
choyai 7:8d69b4230a02 91 }
choyai 7:8d69b4230a02 92 if (moveq3 == true) {
choyai 7:8d69b4230a02 93 if (q3_count >= q3_step) {
choyai 7:8d69b4230a02 94 moveq3 = false;
choyai 7:8d69b4230a02 95 if (DR_1 == 0) {
choyai 7:8d69b4230a02 96 q3 += q3_count / 22.2222222;
choyai 7:8d69b4230a02 97 } else {
choyai 7:8d69b4230a02 98 q3 -= q3_count / 22.2222222;
choyai 7:8d69b4230a02 99 }
choyai 7:8d69b4230a02 100 q3_count = 0;
choyai 7:8d69b4230a02 101 PUL_1 = 0;
choyai 7:8d69b4230a02 102 } else {
choyai 7:8d69b4230a02 103 if ((uint8_t)q3_count % (uint8_t)q3_speed == 0) {
choyai 7:8d69b4230a02 104 PUL_1 = !PUL_1;
choyai 7:8d69b4230a02 105 q3_count++;
choyai 7:8d69b4230a02 106 }
choyai 7:8d69b4230a02 107 // q3_count++;
pbdt1997 5:d86a0c29dd29 108 }
choyai 7:8d69b4230a02 109 }
choyai 7:8d69b4230a02 110 }
choyai 7:8d69b4230a02 111
choyai 7:8d69b4230a02 112 void drvStepper1(double angle, double speed) {
choyai 7:8d69b4230a02 113 q3_speed = speed;
choyai 7:8d69b4230a02 114 if (angle - q3 >= 0) {
choyai 7:8d69b4230a02 115 DR_1 = 0;
choyai 7:8d69b4230a02 116 } else {
choyai 7:8d69b4230a02 117 DR_1 = 1;
choyai 7:8d69b4230a02 118 }
choyai 7:8d69b4230a02 119 convertStep(angle, 3);
choyai 7:8d69b4230a02 120 moveq3 = true;
choyai 7:8d69b4230a02 121 }
choyai 7:8d69b4230a02 122
choyai 7:8d69b4230a02 123 void drvStepper2(double angle, double speed) {
choyai 7:8d69b4230a02 124 // stepper.detach();
choyai 7:8d69b4230a02 125 // stepper.attach_us(&driveStepper, 1000000/(speed));
choyai 7:8d69b4230a02 126 q4_speed = speed;
choyai 7:8d69b4230a02 127 if (angle - q4 >= 0) {
choyai 7:8d69b4230a02 128 DR_2 = 0;
choyai 7:8d69b4230a02 129 } else {
choyai 7:8d69b4230a02 130 DR_2 = 1;
choyai 7:8d69b4230a02 131 }
choyai 7:8d69b4230a02 132 convertStep(angle, 4);
choyai 7:8d69b4230a02 133 moveq4 = true;
choyai 7:8d69b4230a02 134 }
choyai 7:8d69b4230a02 135
choyai 7:8d69b4230a02 136 void veloControl(double q3_trgt, double q4_trgt, double v3, double v4) {
choyai 7:8d69b4230a02 137 // wait till previous movement stopped
choyai 7:8d69b4230a02 138 while (moveq4 || moveq3) {
choyai 7:8d69b4230a02 139 }
choyai 7:8d69b4230a02 140 drvStepper1(q3_trgt, v3);
choyai 7:8d69b4230a02 141 drvStepper2(q4_trgt, v4);
choyai 7:8d69b4230a02 142 moveq3 = true;
choyai 7:8d69b4230a02 143 moveq4 = true;
pbdt1997 5:d86a0c29dd29 144 }
pbdt1997 5:d86a0c29dd29 145
choyai 7:8d69b4230a02 146 void setHome() {
choyai 7:8d69b4230a02 147 while (prox1_flag == false) {
choyai 7:8d69b4230a02 148 drvStepper1(-0.5, 10);
choyai 7:8d69b4230a02 149 // drvStepper2(1, 10);
choyai 7:8d69b4230a02 150 // q3 = 0;
choyai 7:8d69b4230a02 151 // printf("driving\n");
choyai 7:8d69b4230a02 152 // q4 = 0;
choyai 7:8d69b4230a02 153 }
choyai 7:8d69b4230a02 154 prox1_flag = false;
choyai 7:8d69b4230a02 155 veloControl(30, 0, 10, 100);
choyai 7:8d69b4230a02 156 // q3 = 0;
choyai 7:8d69b4230a02 157 q4 = 0;
pbdt1997 5:d86a0c29dd29 158 }
pbdt1997 5:d86a0c29dd29 159
choyai 7:8d69b4230a02 160 // Communication Routines
choyai 7:8d69b4230a02 161
choyai 7:8d69b4230a02 162 // store data from pc
choyai 7:8d69b4230a02 163 void SM_RxD(int c) {
choyai 7:8d69b4230a02 164 if (getPackage == 0) {
choyai 7:8d69b4230a02 165 if (SM_id < 2) {
choyai 7:8d69b4230a02 166 if (c == 255) {
choyai 7:8d69b4230a02 167 array[SM_id] = c;
choyai 7:8d69b4230a02 168 SM_id++;
choyai 7:8d69b4230a02 169 } else {
choyai 7:8d69b4230a02 170 SM_id = 0;
choyai 7:8d69b4230a02 171 }
choyai 7:8d69b4230a02 172 } else if (SM_id == 2) {
choyai 7:8d69b4230a02 173 array[SM_id] = c;
choyai 7:8d69b4230a02 174 command_ID = c;
choyai 7:8d69b4230a02 175 SM_id++;
choyai 7:8d69b4230a02 176 } else if (SM_id > 2) {
choyai 7:8d69b4230a02 177 array[SM_id] = c;
choyai 7:8d69b4230a02 178 if (SM_id >= 9) {
choyai 7:8d69b4230a02 179 getPackage = 1;
choyai 7:8d69b4230a02 180 SM_id = 0;
choyai 7:8d69b4230a02 181 } else {
choyai 7:8d69b4230a02 182 SM_id++;
choyai 7:8d69b4230a02 183 }
pbdt1997 5:d86a0c29dd29 184 }
choyai 7:8d69b4230a02 185 }
choyai 7:8d69b4230a02 186 }
choyai 7:8d69b4230a02 187 // performs checksum
choyai 7:8d69b4230a02 188 int sumCheck(char arr) {
choyai 7:8d69b4230a02 189 char sum = 0;
choyai 7:8d69b4230a02 190 char checksum = arr[9];
choyai 7:8d69b4230a02 191 for (int i = 0; i < 9; i++) {
choyai 7:8d69b4230a02 192 sum = sum + (char)arr[i];
choyai 7:8d69b4230a02 193 }
choyai 7:8d69b4230a02 194 sum = (char)sum;
choyai 7:8d69b4230a02 195 if (sum == checksum) {
choyai 7:8d69b4230a02 196 return 1;
choyai 7:8d69b4230a02 197 } else {
choyai 7:8d69b4230a02 198 return 0;
choyai 7:8d69b4230a02 199 }
pbdt1997 5:d86a0c29dd29 200 }
pbdt1997 5:d86a0c29dd29 201
choyai 7:8d69b4230a02 202 // receive data from pc
choyai 7:8d69b4230a02 203 void RX_INT() {
choyai 7:8d69b4230a02 204 int data = pc.getc();
choyai 7:8d69b4230a02 205 SM_RxD(data);
pbdt1997 5:d86a0c29dd29 206 }
pbdt1997 5:d86a0c29dd29 207
choyai 7:8d69b4230a02 208 // store data from master
choyai 7:8d69b4230a02 209 void Slave_Rx(int c) {
choyai 7:8d69b4230a02 210 if (SPIgot == 0) {
choyai 7:8d69b4230a02 211 if (data_id < 2) {
choyai 7:8d69b4230a02 212 if (c == 255) {
choyai 7:8d69b4230a02 213 SPIarr[data_id] = c;
choyai 7:8d69b4230a02 214 slave.reply(0x00);
choyai 7:8d69b4230a02 215 data_id++;
choyai 7:8d69b4230a02 216 } else {
choyai 7:8d69b4230a02 217 data_id = 0;
choyai 7:8d69b4230a02 218 slave.reply(0x01);
choyai 7:8d69b4230a02 219 }
choyai 7:8d69b4230a02 220 } else if (data_id == 2) {
choyai 7:8d69b4230a02 221 SPIarr[data_id] = c;
choyai 7:8d69b4230a02 222 spiComID = c;
choyai 7:8d69b4230a02 223 slave.reply(0x00);
choyai 7:8d69b4230a02 224 data_id++;
choyai 7:8d69b4230a02 225 } else if (data_id > 2) {
choyai 7:8d69b4230a02 226 SPIarr[data_id] = c;
choyai 7:8d69b4230a02 227 if (data_id >= 9) {
choyai 7:8d69b4230a02 228 SPIgot = 1;
choyai 7:8d69b4230a02 229 data_id = 0;
choyai 7:8d69b4230a02 230 } else {
choyai 7:8d69b4230a02 231 slave.reply(0x00);
choyai 7:8d69b4230a02 232 data_id++;
choyai 7:8d69b4230a02 233 }
pbdt1997 6:ce7c403ad32a 234 }
choyai 7:8d69b4230a02 235 }
pbdt1997 5:d86a0c29dd29 236 }
pbdt1997 5:d86a0c29dd29 237
pbdt1997 4:fb0905390ebc 238 int main() {
choyai 7:8d69b4230a02 239 stepper.attach_us(&driveStepper, 1000.0);
choyai 7:8d69b4230a02 240 pc.baud(250000);
choyai 7:8d69b4230a02 241 prox1.fall(&stop_q3);
choyai 7:8d69b4230a02 242 prox2.fall(&stop_q4);
choyai 7:8d69b4230a02 243 slave.format(8, 3);
choyai 7:8d69b4230a02 244 slave.frequency(1000000);
choyai 7:8d69b4230a02 245 moveq4 = false;
choyai 7:8d69b4230a02 246 moveq3 = false;
choyai 7:8d69b4230a02 247 SM_id = 0;
choyai 7:8d69b4230a02 248 data_id = 0;
choyai 7:8d69b4230a02 249 // setHome();
choyai 7:8d69b4230a02 250 // wait(2);
choyai 7:8d69b4230a02 251 q3 = 0;
choyai 7:8d69b4230a02 252 q4 = 0;
pbdt1997 6:ce7c403ad32a 253
choyai 7:8d69b4230a02 254 // veloControl(120,0,10,10);
choyai 7:8d69b4230a02 255 // printf("q3 = %.2f q4 = %.2f\n", q3, q4);
choyai 7:8d69b4230a02 256 // while(moveq4||moveq3){
choyai 7:8d69b4230a02 257 // printf("numba 1 q3_count %.2f q3 %.2f\n",q3_count,q3);
choyai 7:8d69b4230a02 258 // }
choyai 7:8d69b4230a02 259
choyai 7:8d69b4230a02 260 while (1) {
choyai 7:8d69b4230a02 261 if (slave.receive()) {
choyai 7:8d69b4230a02 262 char c = slave.read();
choyai 7:8d69b4230a02 263 SlaveRx(c);
choyai 7:8d69b4230a02 264 }
choyai 7:8d69b4230a02 265 if (SPIgot >= 1) {
choyai 7:8d69b4230a02 266 int spi_received = sumCheck(SPIarr);
choyai 7:8d69b4230a02 267 if (!received) {
choyai 7:8d69b4230a02 268 slave.reply(0x002);
choyai 7:8d69b4230a02 269 } else {
choyai 7:8d69b4230a02 270 switch (SPIarr[2]) {
choyai 7:8d69b4230a02 271 default:
choyai 7:8d69b4230a02 272 pc.printf("received successfully");
choyai 7:8d69b4230a02 273 break;
pbdt1997 5:d86a0c29dd29 274 }
choyai 7:8d69b4230a02 275 }
choyai 7:8d69b4230a02 276 }
choyai 7:8d69b4230a02 277 if (getPackage >= 1) {
choyai 7:8d69b4230a02 278 int received = sumCheck(array);
choyai 7:8d69b4230a02 279 if (!received) {
choyai 7:8d69b4230a02 280 pc.printf("resend");
choyai 7:8d69b4230a02 281 getPackage = 0;
choyai 7:8d69b4230a02 282 } else {
choyai 7:8d69b4230a02 283 switch (array[2]) {
choyai 7:8d69b4230a02 284 case 0:
choyai 7:8d69b4230a02 285 setHome();
choyai 7:8d69b4230a02 286 case 1:
choyai 7:8d69b4230a02 287 pc.printf("q3 = %.2f, q4 = %.2f\n", q3, q4);
choyai 7:8d69b4230a02 288 pc.printf("done");
choyai 7:8d69b4230a02 289 getPackage = 0;
choyai 7:8d69b4230a02 290 break;
choyai 7:8d69b4230a02 291 default:
choyai 7:8d69b4230a02 292 pc.printf("resend");
choyai 7:8d69b4230a02 293 getPackage = 0;
choyai 7:8d69b4230a02 294 break;
pbdt1997 6:ce7c403ad32a 295 }
choyai 7:8d69b4230a02 296 }
choyai 7:8d69b4230a02 297 }
choyai 7:8d69b4230a02 298 }
pbdt1997 6:ce7c403ad32a 299
choyai 7:8d69b4230a02 300 // printf("q3 = %.2f q4 = %.2f\n", q3, q4);
choyai 7:8d69b4230a02 301 // veloControl(-10,-15,10,10);
choyai 7:8d69b4230a02 302 //
choyai 7:8d69b4230a02 303 // while(moveq4||moveq3){
choyai 7:8d69b4230a02 304 // printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
choyai 7:8d69b4230a02 305 // }
choyai 7:8d69b4230a02 306 // printf("q3 = %.2f q4 = %.2f\n", q3, q4);
choyai 7:8d69b4230a02 307 // while(1){
choyai 7:8d69b4230a02 308 //// drvStepper2(60.0, 1000);
choyai 7:8d69b4230a02 309 //// wait(5);
choyai 7:8d69b4230a02 310 //// drvStepper2(30.0, 1000);
choyai 7:8d69b4230a02 311 // printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
choyai 7:8d69b4230a02 312 // wait_ms(10);
choyai 7:8d69b4230a02 313 //
choyai 7:8d69b4230a02 314 // }
pbdt1997 0:a3b83d874092 315 }