main.cpp

00001 #include "mbed.h"
00002 
00003 InterruptIn prox1(PA_14);
00004 InterruptIn prox2(PA_13);
00005 // DigitalIn prox2(PA_14);
00006 Ticker stepper;
00007 // Ticker stepper2;
00008 // DigitalOut led(LED1); //on-board led
00009 Serial pc(USBTX, USBRX);
00010 
00011 SPISlave slave(PA_7, PA_6, PA_5, PA_15); // MOSI MISO CLK CS
00012 
00013 // Stepper Motor 1
00014 DigitalOut PUL_1(D3);
00015 DigitalOut DR_1(PC_0);
00016 
00017 // Stepper Motor 2
00018 DigitalOut PUL_2(D4);
00019 DigitalOut DR_2(PC_1);
00020 
00021 // globals
00022 char array[20];
00023 char SPIarr[20];
00024 int SPIgot = 0;
00025 char spiComID;
00026 char SM_id = 0, data_id = 0;
00027 int getPackage = 0;
00028 char command_ID;
00029 char arraySize = sizeof(array);
00030 char getData = 0;
00031 char checkData = 0;
00032 double q3 = 0, q4 = 0;
00033 double q3_count = 0, q4_count = 0;
00034 double q3_speed = 1, q4_speed = 1;
00035 double q3_step = 0, q4_step = 0;
00036 bool moveq3 = false, moveq4 = false;
00037 bool stop = false;
00038 bool prox1_flag = false;
00039 bool prox2_flag = false;
00040 double t = 10000.0;
00041 
00042 // Functions
00043 
00044 // conversion funcs
00045 void convertStep(double angle, char joint) {
00046   if (joint == 3) {
00047     q3_step = angle - q3;
00048     q3_step = abs(q3_step * (22.2222222));
00049   } else if (joint == 4) {
00050     q4_step = angle - q4;
00051     //        q4_step = q4_step*(10.0 + 1.0/9.0);
00052     q4_step = abs(q4_step * (11.1111111));
00053   }
00054 }
00055 
00056 // ISRs//
00057 
00058 // Proximity sensors
00059 void stop_q3() {
00060   PUL_1 = 0;
00061   prox1_flag = true;
00062   moveq3 = false;
00063   //        led = !led;
00064 }
00065 
00066 void stop_q4() {
00067   PUL_2 = 0;
00068   prox2_flag = true;
00069   moveq4 = false;
00070 }
00071 
00072 //
00073 void driveStepper() {
00074   if (moveq4 == true) {
00075     if (q4_count >= q4_step) {
00076       moveq4 = false;
00077       if (DR_2 == 0) {
00078         q4 += q4_count / 11.1111111;
00079       } else {
00080         q4 -= q4_count / 11.1111111;
00081       }
00082       q4_count = 0;
00083       PUL_2 = 0;
00084 
00085     } else {
00086       if ((uint8_t)q4_count % (uint8_t)q4_speed == 0) {
00087         PUL_2 = !PUL_2;
00088         q4_count++;
00089       }
00090     }
00091   }
00092   if (moveq3 == true) {
00093     if (q3_count >= q3_step) {
00094       moveq3 = false;
00095       if (DR_1 == 0) {
00096         q3 += q3_count / 22.2222222;
00097       } else {
00098         q3 -= q3_count / 22.2222222;
00099       }
00100       q3_count = 0;
00101       PUL_1 = 0;
00102     } else {
00103       if ((uint8_t)q3_count % (uint8_t)q3_speed == 0) {
00104         PUL_1 = !PUL_1;
00105         q3_count++;
00106       }
00107       //            q3_count++;
00108     }
00109   }
00110 }
00111 
00112 void drvStepper1(double angle, double speed) {
00113   q3_speed = speed;
00114   if (angle - q3 >= 0) {
00115     DR_1 = 0;
00116   } else {
00117     DR_1 = 1;
00118   }
00119   convertStep(angle, 3);
00120   moveq3 = true;
00121 }
00122 
00123 void drvStepper2(double angle, double speed) {
00124   //    stepper.detach();
00125   //    stepper.attach_us(&driveStepper, 1000000/(speed));
00126   q4_speed = speed;
00127   if (angle - q4 >= 0) {
00128     DR_2 = 0;
00129   } else {
00130     DR_2 = 1;
00131   }
00132   convertStep(angle, 4);
00133   moveq4 = true;
00134 }
00135 
00136 void veloControl(double q3_trgt, double q4_trgt, double v3, double v4) {
00137   // wait till previous movement stopped
00138   while (moveq4 || moveq3) {
00139   }
00140   drvStepper1(q3_trgt, v3);
00141   drvStepper2(q4_trgt, v4);
00142   moveq3 = true;
00143   moveq4 = true;
00144 }
00145 
00146 void setHome() {
00147   while (prox1_flag == false) {
00148     drvStepper1(-0.5, 10);
00149     //        drvStepper2(1, 10);
00150     //        q3 = 0;
00151     //        printf("driving\n");
00152     //        q4 = 0;
00153   }
00154   prox1_flag = false;
00155   veloControl(30, 0, 10, 100);
00156   //    q3 = 0;
00157   q4 = 0;
00158 }
00159 
00160 // Communication Routines
00161 
00162 // store data from pc
00163 void SM_RxD(int c) {
00164   if (getPackage == 0) {
00165     if (SM_id < 2) {
00166       if (c == 255) {
00167         array[SM_id] = c;
00168         SM_id++;
00169       } else {
00170         SM_id = 0;
00171       }
00172     } else if (SM_id == 2) {
00173       array[SM_id] = c;
00174       command_ID = c;
00175       SM_id++;
00176     } else if (SM_id > 2) {
00177       array[SM_id] = c;
00178       if (SM_id >= 9) {
00179         getPackage = 1;
00180         SM_id = 0;
00181       } else {
00182         SM_id++;
00183       }
00184     }
00185   }
00186 }
00187 // performs checksum
00188 int sumCheck(char arr) {
00189   char sum = 0;
00190   char checksum = arr[9];
00191   for (int i = 0; i < 9; i++) {
00192     sum = sum + (char)arr[i];
00193   }
00194   sum = (char)sum;
00195   if (sum == checksum) {
00196     return 1;
00197   } else {
00198     return 0;
00199   }
00200 }
00201 
00202 // receive data from pc
00203 void RX_INT() {
00204   int data = pc.getc();
00205   SM_RxD(data);
00206 }
00207 
00208 // store data from master
00209 void Slave_Rx(int c) {
00210   if (SPIgot == 0) {
00211     if (data_id < 2) {
00212       if (c == 255) {
00213         SPIarr[data_id] = c;
00214         slave.reply(0x00);
00215         data_id++;
00216       } else {
00217         data_id = 0;
00218         slave.reply(0x01);
00219       }
00220     } else if (data_id == 2) {
00221       SPIarr[data_id] = c;
00222       spiComID = c;
00223       slave.reply(0x00);
00224       data_id++;
00225     } else if (data_id > 2) {
00226       SPIarr[data_id] = c;
00227       if (data_id >= 9) {
00228         SPIgot = 1;
00229         data_id = 0;
00230       } else {
00231         slave.reply(0x00);
00232         data_id++;
00233       }
00234     }
00235   }
00236 }
00237 
00238 int main() {
00239   stepper.attach_us(&driveStepper, 1000.0);
00240   pc.baud(250000);
00241   prox1.fall(&stop_q3);
00242   prox2.fall(&stop_q4);
00243   slave.format(8, 3);
00244   slave.frequency(1000000);
00245   moveq4 = false;
00246   moveq3 = false;
00247   SM_id = 0;
00248   data_id = 0;
00249   //    setHome();
00250   //    wait(2);
00251   q3 = 0;
00252   q4 = 0;
00253 
00254   //    veloControl(120,0,10,10);
00255   //    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
00256   //    while(moveq4||moveq3){
00257   //        printf("numba 1 q3_count %.2f q3 %.2f\n",q3_count,q3);
00258   //        }
00259 
00260   while (1) {
00261     if (slave.receive()) {
00262       char c = slave.read();
00263       SlaveRx(c);
00264     }
00265     if (SPIgot >= 1) {
00266       int spi_received = sumCheck(SPIarr);
00267       if (!received) {
00268         slave.reply(0x002);
00269       } else {
00270         switch (SPIarr[2]) {
00271         default:
00272           pc.printf("received successfully");
00273           break;
00274         }
00275       }
00276     }
00277     if (getPackage >= 1) {
00278       int received = sumCheck(array);
00279       if (!received) {
00280         pc.printf("resend");
00281         getPackage = 0;
00282       } else {
00283         switch (array[2]) {
00284         case 0:
00285           setHome();
00286         case 1:
00287           pc.printf("q3 = %.2f, q4 = %.2f\n", q3, q4);
00288           pc.printf("done");
00289           getPackage = 0;
00290           break;
00291         default:
00292           pc.printf("resend");
00293           getPackage = 0;
00294           break;
00295         }
00296       }
00297     }
00298   }
00299 
00300   //    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
00301   //    veloControl(-10,-15,10,10);
00302   //
00303   //       while(moveq4||moveq3){
00304   //        printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
00305   //        }
00306   //    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
00307   //   while(1){
00308   ////        drvStepper2(60.0, 1000);
00309   ////        wait(5);
00310   ////        drvStepper2(30.0, 1000);
00311   //        printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
00312   //        wait_ms(10);
00313   //
00314   //    }
00315 }