Robotics Studio
/
NucleoBoard_1
Board1
main.cpp
- Committer:
- pbdt1997
- Date:
- 2019-03-27
- Revision:
- 0:97879c8efe59
- Child:
- 1:4eeae0f92c4c
File content as of revision 0:97879c8efe59:
//Module8-9 //Nucleo Board 1 (Master) #include "mbed.h" #include <Map.hpp> 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 DigitalOut csMstr(PA_15); //SPI1_SSEL Master DigitalOut csEnc1(PB_12); //SPI2_SSEL Encoder 1 DigitalOut csEnc2(PA_4); //SPI3_SSEL Encoder 2 //Parameters char array[4]; char arraySize = sizeof(array); char getData = 0; char checkData = 0; char encArr[2]= {255,255}; char encLength = 2; bool start = false; ///////////////////////////// //Functions void RX(int data){ array[checkData] = data; checkData++; if(checkData == arraySize){ getData = 1; checkData = 0; } } void RX_INT(){ int data = pc.getc(); RX(data); } float ConvertAngle(int encData){ Map map(0, 16383, 0, 359); float angle = map.Calculate(encData); return angle; } float readEnc1(){ //Encoder1 = LOW csEnc1 = 0; char *arrAdd = &encArr[0]; int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2); encData = encArr[0] * 256 + encArr[1]; csEnc1 = 1; return ConvertAngle(encData/2); } float readEnc2(){ //Encoder2 = LOW 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); } //Main int main() { pc.baud(9600); RX_INT(); pc.attach(&RX_INT, Serial::RxIrq); csMstr = 1; csEnc1 = 1; csEnc2 = 1; master.format(8,3); master.frequency(1000000); mstrEnc1.format(14, 3); mstrEnc1.frequency(500000); 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' start = true; 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); } getData = 0; } if(start == true){ float encData1 = readEnc1(); float encData2 = readEnc2(); printf("Encoder1 = %.2f ", encData1); printf("Encoder2 = %.2f \n\r", encData2); } } }