Diff: main.cpp

Revision:

1:d935f9ed6be5

Parent:

0:eac841605c08

diff -r eac841605c08 -r d935f9ed6be5 main.cpp
--- a/main.cpp	Mon Mar 18 12:20:30 2019 +0000
+++ b/main.cpp	Sat Apr 20 09:56:01 2019 +0000
@@ -4,9 +4,11 @@
 //Variable Declarations
 
 Serial pc(USBTX, USBRX);
+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 spi(D11, D12, D13);//MOSI, MISO, SCLK
-DigitalOut encode1(D10);//Encoder 1
+DigitalOut csEnc1(PB_12);           //SPI2_SSEL Encoder 1
+DigitalOut csEnc2(PA_4);            //SPI3_SSEL Encoder 2
 
 char rx_in = 0, rx_out = 0;
 
@@ -42,19 +44,23 @@
     return angle;
     }
 
-float ReadEncoder(int encid){
-    if(encid == 1){
-        encode1 = 0;
-    }   
-else{
-    //encode2 = 0;
-    }
+//read encoder of joint 1
+float readEnc1(){
+    csEnc1 = 0;
     char *arrAdd = &encArr[0];
-    int encData = spi.write(arrAdd, 2, arrAdd, 2);
-  //  printf("encoder reads %d \n\r", encData);
+    int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
     encData = encArr[0] * 256 + encArr[1];
-    encode1 = 1;
-    printf("encoder reads %d \n\r", encData);
+    csEnc1 = 1;
+    return ConvertAngle(encData/2);
+}
+
+//read encoder of joint 2
+float readEnc2(){
+    csEnc2 = 0;
+    char *arrAdd = &encArr[0];
+    int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
+    encData = encArr[0] * 256 + encArr[1];
+    csEnc2 = 1;
     return ConvertAngle(encData/2);
 }
 
@@ -64,13 +70,16 @@
 //Main
 
 int main() {
-    pc.baud(250000);
+    pc.baud(9600);
     pc.attach(&RX_INT, Serial::RxIrq);
     
-    encode1 = 1;
+    csEnc1 = 1;
+    csEnc2 = 1;
     
-    spi.format(14, 3);
-    spi.frequency(500000);
+    mstrEnc1.format(14, 3);
+    mstrEnc1.frequency(500000);
+    mstrEnc2.format(14, 3);
+    mstrEnc2.frequency(500000);
     
     
 //Program Loop    
@@ -83,8 +92,9 @@
            // pc.putc('A');
           //  getData = 0;
         //}
-        float encdat = ReadEncoder(1);
-        printf("angle = %.2f \n\r", encdat);
+        float encdat1 = readEnc1();
+        float encdat2 = readEnc2();
+        printf("enc1 = %.2f, enc2 = %.2f \n\r", encdat1, encdat2);
     }