main.cpp

00001 #include "mbed.h"              
00002 #include <Map.hpp>
00003 
00004 //Variable Declarations
00005 
00006 Serial pc(USBTX, USBRX);
00007 SPI mstrEnc1(PB_15, PB_14, PB_13);  //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
00008 SPI mstrEnc2(PC_12, PC_11, PC_10);  //SPI3_MOSI, SPI3_MISO, SPI3_SCLK
00009 
00010 DigitalOut csEnc1(PB_12);           //SPI2_SSEL Encoder 1
00011 DigitalOut csEnc2(PA_4);            //SPI3_SSEL Encoder 2
00012 
00013 char rx_in = 0, rx_out = 0;
00014 
00015 char dataSize = 5;
00016 char array[5];
00017 char arraySize = sizeof(array);
00018 char getData = 0;
00019 char checkData = 0;
00020 char encArr[2]= {255,255};
00021 char encLength = 2;
00022 
00023 /////////////////////////////
00024 
00025 //Functions
00026 
00027 void RX(int data){
00028     array[checkData] = data;
00029     checkData++;
00030     if(checkData == arraySize){
00031         getData = 1;
00032         checkData = 0;
00033     }
00034 }
00035 
00036 void RX_INT(){
00037     int data = pc.getc();
00038     RX(data);
00039 }
00040 
00041 float ConvertAngle(int encData){
00042     Map map(0, 16383, 0, 359);
00043     float angle = map.Calculate(encData);
00044     return angle;
00045     }
00046 
00047 //read encoder of joint 1
00048 float readEnc1(){
00049     csEnc1 = 0;
00050     char *arrAdd = &encArr[0];
00051     int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
00052     encData = encArr[0] * 256 + encArr[1];
00053     csEnc1 = 1;
00054     return ConvertAngle(encData/2);
00055 }
00056 
00057 //read encoder of joint 2
00058 float readEnc2(){
00059     csEnc2 = 0;
00060     char *arrAdd = &encArr[0];
00061     int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
00062     encData = encArr[0] * 256 + encArr[1];
00063     csEnc2 = 1;
00064     return ConvertAngle(encData/2);
00065 }
00066 
00067 
00068 void RX_INT();
00069 
00070 //Main
00071 
00072 int main() {
00073     pc.baud(9600);
00074     pc.attach(&RX_INT, Serial::RxIrq);
00075     
00076     csEnc1 = 1;
00077     csEnc2 = 1;
00078     
00079     mstrEnc1.format(14, 3);
00080     mstrEnc1.frequency(500000);
00081     mstrEnc2.format(14, 3);
00082     mstrEnc2.frequency(500000);
00083     
00084     
00085 //Program Loop    
00086     while(true){
00087         //if(getData == 1){
00088           //  for(char i = 0; i < arraySize; i++){
00089 //              printf("array[%d] = %d\n", i,array[i]);
00090             //    pc.putc(array[i]);
00091             //}
00092            // pc.putc('A');
00093           //  getData = 0;
00094         //}
00095         float encdat1 = readEnc1();
00096         float encdat2 = readEnc2();
00097         printf("enc1 = %.2f, enc2 = %.2f \n\r", encdat1, encdat2);
00098     }
00099     
00100     
00101     
00102     
00103 }