Diff: main.cpp

Revision:

1:4eeae0f92c4c

Parent:

0:97879c8efe59

Child:

2:2ef793d0ac01

--- a/main.cpp	Wed Mar 27 14:09:53 2019 +0000
+++ b/main.cpp	Sat Apr 20 07:02:28 2019 +0000
@@ -7,13 +7,18 @@
 Serial pc(USBTX, USBRX);
 
 SPI master(PA_7, PA_6, PA_5);       //SPI1_MOSI, SPI1_MISO, SPI1_SCLK
-SPI mstrEnc1(PB_15, PB_14, PB_13);  //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
-SPI mstrEnc2(PC_12, PC_11, PC_10);     //SPI3_MOSI, SPI3_MISO, SPI3_SCLK
+SPI mstrEnc2(PB_15, PB_14, PB_13);  //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
+SPI mstrEnc1(PC_12, PC_11, PC_10);  //SPI3_MOSI, SPI3_MISO, SPI3_SCLK
 
 DigitalOut csMstr(PA_15);           //SPI1_SSEL Master
-DigitalOut csEnc1(PB_12);           //SPI2_SSEL Encoder 1
-DigitalOut csEnc2(PA_4);            //SPI3_SSEL Encoder 2
+DigitalOut csEnc2(PB_12);           //SPI2_SSEL Encoder 1
+DigitalOut csEnc1(PA_4);            //SPI3_SSEL Encoder 2
 
+DigitalOut dir1(PC_0);
+PwmOut pwm1(PC_9);
+
+DigitalOut dir2(PC_1);
+PwmOut pwm2(PC_8);
 
 //Parameters
 
@@ -25,10 +30,12 @@
 char encLength = 2;
 bool start = false;
 
+
 /////////////////////////////
 
 //Functions
 
+//store data from pc
 void RX(int data){
     array[checkData] = data;
     checkData++;
@@ -38,19 +45,21 @@
     }
 }
 
+//receive data from pc
 void RX_INT(){
     int data = pc.getc();
     RX(data);
 }
 
+//convert 14bit data to angle
 float ConvertAngle(int encData){
     Map map(0, 16383, 0, 359);
     float angle = map.Calculate(encData);
     return angle;
     }
 
+//read encoder of joint 1
 float readEnc1(){
-    //Encoder1 = LOW
     csEnc1 = 0;
     char *arrAdd = &encArr[0];
     int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
@@ -59,8 +68,8 @@
     return ConvertAngle(encData/2);
 }
 
+//read encoder of joint 2
 float readEnc2(){
-    //Encoder2 = LOW
     csEnc2 = 0;
     char *arrAdd = &encArr[0];
     int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
@@ -69,10 +78,112 @@
     return ConvertAngle(encData/2);
 }
 
+//drive motor joint1
+void drvMotor1(float duty){
+    if(duty > 0.9){
+        duty = 0.9;
+    }
+    else if(duty < -0.9){
+        duty = -0.9;
+    }
+    if(duty >= 0){
+        dir1 = 1; //CW
+        
+    }
+    else if(duty < 0){
+        dir1 = 0; //CCW
+        duty = -duty;
+    }
+    pwm1.period_ms(1.0f);
+    pwm1.write(duty);
+}
+
+//drive motor joint1
+void drvMotor2(float duty){
+    if(duty > 0.5){
+        duty = 0.5;
+    }
+    else if(duty < -0.5){
+        duty = -0.5;
+    }
+    if(duty >= 0){
+        dir2 = 1; //CCW
+    }
+    else if(duty < 0){
+        dir2 = 0; //CW
+        duty = -duty;
+    }
+    pwm2.period_ms(1.0f);
+    pwm2.write(duty);
+} 
+
+//PID of joint 1
+void PID_1(float pos){
+    pos = -pos;
+    const float Kp=0.5,Ki=0,Kd=0.1;
+    float u,e,p,s;
+    float offset = 110.66;
+    bool stop1 = false;
+    
+    while(stop1==false){
+        float encData1 = readEnc1();
+        float encData2 = readEnc2();
+        printf("Encoder1: %.2f  ", encData1);
+        printf("Encoder2: %.2f\n", encData2);
+        if(encData1 < 110){
+            encData1 = encData1 + 360;
+        }
+        e = (pos - encData1) + offset;
+        s = s + e;
+        if(abs(e)>0.5){
+            u = (Kp*e) + (Ki*s) + (Kd*(e-p));
+        }
+        else{
+            u = 0;
+            drvMotor1(0);
+            stop1 = true;
+        }
+        drvMotor1(u);
+        p = e;
+    }
+}
+
+//PID of joint 2
+void PID_2(float pos){
+    const float Kp=1,Ki=0,Kd=0.1;
+    float u,e,p,s;
+    float offset = 21.52 - 135;
+    bool stop = false;
+    pos=-pos;
+    while(stop==false){
+        float encData1 = readEnc1();
+        float encData2 = readEnc2();
+        printf("Encoder1: %.2f  ", encData1);
+        printf("Encoder2: %.2f\n", encData2);
+        e = (pos - encData2) + offset;
+        s = s + e;
+        if(abs(e)>0.5){
+            u = (Kp*e) + (Ki*s) + (Kd*(e-p));
+        }
+        else{
+            u = 0;
+            drvMotor2(0);
+            stop = true;
+        }
+        drvMotor2(u);
+        p = e;
+    }
+}
+//Set Home
+void setHome(){
+    PID_1(0);
+    PID_2(0);
+    }
+
 //Main
 
 int main() {
-    pc.baud(9600);
+    pc.baud(256000);
     RX_INT();
     pc.attach(&RX_INT, Serial::RxIrq);
     
@@ -87,16 +198,10 @@
     mstrEnc2.format(14, 3);
     mstrEnc2.frequency(500000);
     
-    
 //Program Loop    
     while(true){
         if(getData == 1){
             for(char i = 0; i < arraySize; i++){
-                //printf("array[%d] = %d\n", i,array[i]);
-//                csMstr = 0;
-//                master.write(array[i]);
-//                int masterData = master.write(0x00);
-//                printf("masterData = %d\n", masterData);
                 pc.putc(array[i]);
             }
             if(array[2] == 122){ //receive 'z'
@@ -104,23 +209,27 @@
                 csMstr = 0;
                 master.write(array[3]);
                 int masterData = master.write(0x00);
-                //printf("masterData = %d\n", masterData);
-                //state = 0;
                 csMstr = 1;
                 pc.putc(masterData);
                 pc.putc('A');
-                
-//                wait(1);
+            }
+            if(array[2] == 121){ //'y'
+                setHome();
             }
             
             getData = 0;
         }
-        if(start == true){
-            float encData1 = readEnc1();
-            float encData2 = readEnc2();
-            printf("Encoder1 = %.2f     ", encData1);
-            printf("Encoder2 = %.2f \n\r", encData2);
-        }
+        //test driving joint 1 and joint 2
+              PID_2(45);
+        wait(0.5);
+        PID_1(90);
+        wait(0.1);
+        PID_2(30);
+        wait(3);
+        PID_1(0);
+        wait(0.1);
+        PID_2(-45);
+        wait(100);
         
     }