Revision 7:8d69b4230a02, committed 2019-04-23

Comitter:

choyai

Date:

Tue Apr 23 07:17:41 2019 +0000

Parent:

6:ce7c403ad32a

Commit message:

untested spi

Changed in this revision

--- a/main.cpp	Mon Apr 22 16:15:04 2019 +0000
+++ b/main.cpp	Tue Apr 23 07:17:41 2019 +0000
@@ -2,24 +2,33 @@
 
 InterruptIn prox1(PA_14);
 InterruptIn prox2(PA_13);
-//DigitalIn prox2(PA_14);
+// DigitalIn prox2(PA_14);
 Ticker stepper;
-//Ticker stepper2;
-//DigitalOut led(LED1); //on-board led
+// Ticker stepper2;
+// DigitalOut led(LED1); //on-board led
 Serial pc(USBTX, USBRX);
 
-SPISlave slave(PA_7, PA_6, PA_5, PA_15); //MOSI MISO CLK CS
+SPISlave slave(PA_7, PA_6, PA_5, PA_15); // MOSI MISO CLK CS
 
-//Stepper Motor 1
+// Stepper Motor 1
 DigitalOut PUL_1(D3);
 DigitalOut DR_1(PC_0);
 
-//Stepper Motor 2
+// Stepper Motor 2
 DigitalOut PUL_2(D4);
 DigitalOut DR_2(PC_1);
 
-
-//globals
+// globals
+char array[20];
+char SPIarr[20];
+int SPIgot = 0;
+char spiComID;
+char SM_id = 0, data_id = 0;
+int getPackage = 0;
+char command_ID;
+char arraySize = sizeof(array);
+char getData = 0;
+char checkData = 0;
 double q3 = 0, q4 = 0;
 double q3_count = 0, q4_count = 0;
 double q3_speed = 1, q4_speed = 1;
@@ -30,175 +39,277 @@
 bool prox2_flag = false;
 double t = 10000.0;
 
-//Functions
+// Functions
 
-//conversion funcs
-void convertStep(double angle, char joint){
-    if(joint == 3){
-        q3_step = angle - q3;
-        q3_step = abs(q3_step*(22.2222222));
-    }
-    else if(joint == 4){
-        q4_step = angle - q4;
-//        q4_step = q4_step*(10.0 + 1.0/9.0);
-        q4_step = abs(q4_step*(11.1111111));
-    }
+// conversion funcs
+void convertStep(double angle, char joint) {
+  if (joint == 3) {
+    q3_step = angle - q3;
+    q3_step = abs(q3_step * (22.2222222));
+  } else if (joint == 4) {
+    q4_step = angle - q4;
+    //        q4_step = q4_step*(10.0 + 1.0/9.0);
+    q4_step = abs(q4_step * (11.1111111));
+  }
 }
 
-
-//ISRs//
+// ISRs//
 
-//Proximity sensors
-void stop_q3(){
-        PUL_1 = 0;
-        prox1_flag = true;
-        moveq3 = false;
-//        led = !led;
+// Proximity sensors
+void stop_q3() {
+  PUL_1 = 0;
+  prox1_flag = true;
+  moveq3 = false;
+  //        led = !led;
 }
 
-void stop_q4(){
-        PUL_2 = 0;
-        prox2_flag = true;
-        moveq4 = false;
+void stop_q4() {
+  PUL_2 = 0;
+  prox2_flag = true;
+  moveq4 = false;
 }
 
 //
-void driveStepper(){
-    if(moveq4 == true){
-        if(q4_count >= q4_step){
-            moveq4 = false;
-            if(DR_2 == 0){
-                q4 += q4_count/11.1111111;
-                }else{
-                q4 -= q4_count/11.1111111;
-                }
-            q4_count = 0;
-            PUL_2 = 0;
-
-        }
-        else{
-            if((uint8_t)q4_count%(uint8_t)q4_speed==0){
-            PUL_2 = !PUL_2;
-            q4_count++;
-            }
-            
+void driveStepper() {
+  if (moveq4 == true) {
+    if (q4_count >= q4_step) {
+      moveq4 = false;
+      if (DR_2 == 0) {
+        q4 += q4_count / 11.1111111;
+      } else {
+        q4 -= q4_count / 11.1111111;
+      }
+      q4_count = 0;
+      PUL_2 = 0;
 
-        }
+    } else {
+      if ((uint8_t)q4_count % (uint8_t)q4_speed == 0) {
+        PUL_2 = !PUL_2;
+        q4_count++;
+      }
+    }
+  }
+  if (moveq3 == true) {
+    if (q3_count >= q3_step) {
+      moveq3 = false;
+      if (DR_1 == 0) {
+        q3 += q3_count / 22.2222222;
+      } else {
+        q3 -= q3_count / 22.2222222;
+      }
+      q3_count = 0;
+      PUL_1 = 0;
+    } else {
+      if ((uint8_t)q3_count % (uint8_t)q3_speed == 0) {
+        PUL_1 = !PUL_1;
+        q3_count++;
+      }
+      //            q3_count++;
     }
-    if(moveq3 == true){
-        if(q3_count >= q3_step){
-            moveq3 = false;
-            if(DR_1 == 0){
-                q3 += q3_count/22.2222222;
-            }else{
-                q3 -= q3_count/22.2222222;
-            }
-            q3_count = 0;
-            PUL_1 = 0;
-        }
-        else{
-            if((uint8_t)q3_count%(uint8_t)q3_speed==0){
-            PUL_1 = !PUL_1;
-            q3_count++;
-            }
-//            q3_count++;
-        }
-    }
+  }
+}
+
+void drvStepper1(double angle, double speed) {
+  q3_speed = speed;
+  if (angle - q3 >= 0) {
+    DR_1 = 0;
+  } else {
+    DR_1 = 1;
+  }
+  convertStep(angle, 3);
+  moveq3 = true;
+}
+
+void drvStepper2(double angle, double speed) {
+  //    stepper.detach();
+  //    stepper.attach_us(&driveStepper, 1000000/(speed));
+  q4_speed = speed;
+  if (angle - q4 >= 0) {
+    DR_2 = 0;
+  } else {
+    DR_2 = 1;
+  }
+  convertStep(angle, 4);
+  moveq4 = true;
+}
+
+void veloControl(double q3_trgt, double q4_trgt, double v3, double v4) {
+  // wait till previous movement stopped
+  while (moveq4 || moveq3) {
+  }
+  drvStepper1(q3_trgt, v3);
+  drvStepper2(q4_trgt, v4);
+  moveq3 = true;
+  moveq4 = true;
 }
 
-void drvStepper1(double angle, double speed){
-    q3_speed = speed;
-    if(angle - q3 >= 0){
-        DR_1 = 0;
-    }
-    else{
-        DR_1 = 1;
-    }
-    convertStep(angle, 3);
-    moveq3 = true;
+void setHome() {
+  while (prox1_flag == false) {
+    drvStepper1(-0.5, 10);
+    //        drvStepper2(1, 10);
+    //        q3 = 0;
+    //        printf("driving\n");
+    //        q4 = 0;
+  }
+  prox1_flag = false;
+  veloControl(30, 0, 10, 100);
+  //    q3 = 0;
+  q4 = 0;
 }
 
-void drvStepper2(double angle, double speed){
-//    stepper.detach();
-//    stepper.attach_us(&driveStepper, 1000000/(speed));
-    q4_speed = speed;
-    if(angle - q4 >= 0){
-        DR_2 = 0;
+// Communication Routines
+
+// store data from pc
+void SM_RxD(int c) {
+  if (getPackage == 0) {
+    if (SM_id < 2) {
+      if (c == 255) {
+        array[SM_id] = c;
+        SM_id++;
+      } else {
+        SM_id = 0;
+      }
+    } else if (SM_id == 2) {
+      array[SM_id] = c;
+      command_ID = c;
+      SM_id++;
+    } else if (SM_id > 2) {
+      array[SM_id] = c;
+      if (SM_id >= 9) {
+        getPackage = 1;
+        SM_id = 0;
+      } else {
+        SM_id++;
+      }
     }
-    else{
-        DR_2 = 1;
-    }
-    convertStep(angle, 4);
-    moveq4 = true;
+  }
+}
+// performs checksum
+int sumCheck(char arr) {
+  char sum = 0;
+  char checksum = arr[9];
+  for (int i = 0; i < 9; i++) {
+    sum = sum + (char)arr[i];
+  }
+  sum = (char)sum;
+  if (sum == checksum) {
+    return 1;
+  } else {
+    return 0;
+  }
 }
 
-void veloControl(double q3_trgt, double q4_trgt, double v3, double v4){
-    //wait till previous movement stopped
-    while(moveq4||moveq3){
-        }
-    drvStepper1(q3_trgt, v3);
-    drvStepper2(q4_trgt, v4);
-    moveq3 = true;
-    moveq4 = true;
+// receive data from pc
+void RX_INT() {
+  int data = pc.getc();
+  SM_RxD(data);
 }
 
-void setHome(){
-    while(prox1_flag == false){
-        drvStepper1(-0.5, 10);
-//        drvStepper2(1, 10);
-//        q3 = 0;
-//        printf("driving\n");
-//        q4 = 0;
+// store data from master
+void Slave_Rx(int c) {
+  if (SPIgot == 0) {
+    if (data_id < 2) {
+      if (c == 255) {
+        SPIarr[data_id] = c;
+        slave.reply(0x00);
+        data_id++;
+      } else {
+        data_id = 0;
+        slave.reply(0x01);
+      }
+    } else if (data_id == 2) {
+      SPIarr[data_id] = c;
+      spiComID = c;
+      slave.reply(0x00);
+      data_id++;
+    } else if (data_id > 2) {
+      SPIarr[data_id] = c;
+      if (data_id >= 9) {
+        SPIgot = 1;
+        data_id = 0;
+      } else {
+        slave.reply(0x00);
+        data_id++;
+      }
     }
-    prox1_flag = false;
-    veloControl(30, 0, 10, 100);
-//    q3 = 0;
-    q4 = 0;
+  }
 }
 
 int main() {
-    stepper.attach_us(&driveStepper, 1000.0);
-    pc.baud(250000);
-    prox1.fall(&stop_q3);
-    prox2.fall(&stop_q4);
-    moveq4 = false;
-    moveq3 = false;
-//    setHome();
-//    wait(2);
-//    printf("Wtf");
-    q3 = 0;
-    q4 = 0;
+  stepper.attach_us(&driveStepper, 1000.0);
+  pc.baud(250000);
+  prox1.fall(&stop_q3);
+  prox2.fall(&stop_q4);
+  slave.format(8, 3);
+  slave.frequency(1000000);
+  moveq4 = false;
+  moveq3 = false;
+  SM_id = 0;
+  data_id = 0;
+  //    setHome();
+  //    wait(2);
+  q3 = 0;
+  q4 = 0;
 
-    veloControl(120,0,10,10);
-    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
-    while(moveq4||moveq3){
-        printf("numba 1 q3_count %.2f q3 %.2f\n",q3_count,q3);
+  //    veloControl(120,0,10,10);
+  //    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
+  //    while(moveq4||moveq3){
+  //        printf("numba 1 q3_count %.2f q3 %.2f\n",q3_count,q3);
+  //        }
+
+  while (1) {
+    if (slave.receive()) {
+      char c = slave.read();
+      SlaveRx(c);
+    }
+    if (SPIgot >= 1) {
+      int spi_received = sumCheck(SPIarr);
+      if (!received) {
+        slave.reply(0x002);
+      } else {
+        switch (SPIarr[2]) {
+        default:
+          pc.printf("received successfully");
+          break;
         }
-    
-    while(1){
-        //if(prox1 == 0){
-//        printf("......... \n\r");
-//        }
-//        else{
-//            printf("werwerwerewrwer\n");
-//            }
+      }
+    }
+    if (getPackage >= 1) {
+      int received = sumCheck(array);
+      if (!received) {
+        pc.printf("resend");
+        getPackage = 0;
+      } else {
+        switch (array[2]) {
+        case 0:
+          setHome();
+        case 1:
+          pc.printf("q3 = %.2f, q4 = %.2f\n", q3, q4);
+          pc.printf("done");
+          getPackage = 0;
+          break;
+        default:
+          pc.printf("resend");
+          getPackage = 0;
+          break;
         }
-
+      }
+    }
+  }
 
-//    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
-//    veloControl(-10,-15,10,10);
-//
-//       while(moveq4||moveq3){
-//        printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
-//        }
-//    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
- //   while(1){
-////        drvStepper2(60.0, 1000);
-////        wait(5);
-////        drvStepper2(30.0, 1000);
-//        printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
-//        wait_ms(10);
-//
-//    }
-
+  //    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
+  //    veloControl(-10,-15,10,10);
+  //
+  //       while(moveq4||moveq3){
+  //        printf("numba 2 q3 %.2f q4 %.2f\n",q3_count,q4_count);
+  //        }
+  //    printf("q3 = %.2f q4 = %.2f\n", q3, q4);
+  //   while(1){
+  ////        drvStepper2(60.0, 1000);
+  ////        wait(5);
+  ////        drvStepper2(30.0, 1000);
+  //        printf("q4_count %.2f q4 %.2f\n",q4_count,q4);
+  //        wait_ms(10);
+  //
+  //    }
 }