Robotics Studio
/
NucleoBoard_1
Board1
Diff: main.cpp
- Revision:
- 0:97879c8efe59
- Child:
- 1:4eeae0f92c4c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Mar 27 14:09:53 2019 +0000 @@ -0,0 +1,130 @@ +//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); + } + + } + + + + +} \ No newline at end of file