Robotics Studio / Mbed 2 deprecated NucleoBoard_1

Board1

Dependencies:   mbed Map

main.cpp@0:97879c8efe59, 2019-03-27 (annotated)

Committer:
pbdt1997
Date:
Wed Mar 27 14:09:53 2019 +0000
Revision:
0:97879c8efe59
Child:
1:4eeae0f92c4c
27/03/2019: Phase 3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pbdt1997 0:97879c8efe59 1 //Module8-9
pbdt1997 0:97879c8efe59 2 //Nucleo Board 1 (Master)
pbdt1997 0:97879c8efe59 3
pbdt1997 0:97879c8efe59 4 #include "mbed.h"
pbdt1997 0:97879c8efe59 5 #include <Map.hpp>
pbdt1997 0:97879c8efe59 6
pbdt1997 0:97879c8efe59 7 Serial pc(USBTX, USBRX);
pbdt1997 0:97879c8efe59 8
pbdt1997 0:97879c8efe59 9 SPI master(PA_7, PA_6, PA_5); //SPI1_MOSI, SPI1_MISO, SPI1_SCLK
pbdt1997 0:97879c8efe59 10 SPI mstrEnc1(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK
pbdt1997 0:97879c8efe59 11 SPI mstrEnc2(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK
pbdt1997 0:97879c8efe59 12
pbdt1997 0:97879c8efe59 13 DigitalOut csMstr(PA_15); //SPI1_SSEL Master
pbdt1997 0:97879c8efe59 14 DigitalOut csEnc1(PB_12); //SPI2_SSEL Encoder 1
pbdt1997 0:97879c8efe59 15 DigitalOut csEnc2(PA_4); //SPI3_SSEL Encoder 2
pbdt1997 0:97879c8efe59 16
pbdt1997 0:97879c8efe59 17
pbdt1997 0:97879c8efe59 18 //Parameters
pbdt1997 0:97879c8efe59 19
pbdt1997 0:97879c8efe59 20 char array[4];
pbdt1997 0:97879c8efe59 21 char arraySize = sizeof(array);
pbdt1997 0:97879c8efe59 22 char getData = 0;
pbdt1997 0:97879c8efe59 23 char checkData = 0;
pbdt1997 0:97879c8efe59 24 char encArr[2]= {255,255};
pbdt1997 0:97879c8efe59 25 char encLength = 2;
pbdt1997 0:97879c8efe59 26 bool start = false;
pbdt1997 0:97879c8efe59 27
pbdt1997 0:97879c8efe59 28 /////////////////////////////
pbdt1997 0:97879c8efe59 29
pbdt1997 0:97879c8efe59 30 //Functions
pbdt1997 0:97879c8efe59 31
pbdt1997 0:97879c8efe59 32 void RX(int data){
pbdt1997 0:97879c8efe59 33 array[checkData] = data;
pbdt1997 0:97879c8efe59 34 checkData++;
pbdt1997 0:97879c8efe59 35 if(checkData == arraySize){
pbdt1997 0:97879c8efe59 36 getData = 1;
pbdt1997 0:97879c8efe59 37 checkData = 0;
pbdt1997 0:97879c8efe59 38 }
pbdt1997 0:97879c8efe59 39 }
pbdt1997 0:97879c8efe59 40
pbdt1997 0:97879c8efe59 41 void RX_INT(){
pbdt1997 0:97879c8efe59 42 int data = pc.getc();
pbdt1997 0:97879c8efe59 43 RX(data);
pbdt1997 0:97879c8efe59 44 }
pbdt1997 0:97879c8efe59 45
pbdt1997 0:97879c8efe59 46 float ConvertAngle(int encData){
pbdt1997 0:97879c8efe59 47 Map map(0, 16383, 0, 359);
pbdt1997 0:97879c8efe59 48 float angle = map.Calculate(encData);
pbdt1997 0:97879c8efe59 49 return angle;
pbdt1997 0:97879c8efe59 50 }
pbdt1997 0:97879c8efe59 51
pbdt1997 0:97879c8efe59 52 float readEnc1(){
pbdt1997 0:97879c8efe59 53 //Encoder1 = LOW
pbdt1997 0:97879c8efe59 54 csEnc1 = 0;
pbdt1997 0:97879c8efe59 55 char *arrAdd = &encArr[0];
pbdt1997 0:97879c8efe59 56 int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2);
pbdt1997 0:97879c8efe59 57 encData = encArr[0] * 256 + encArr[1];
pbdt1997 0:97879c8efe59 58 csEnc1 = 1;
pbdt1997 0:97879c8efe59 59 return ConvertAngle(encData/2);
pbdt1997 0:97879c8efe59 60 }
pbdt1997 0:97879c8efe59 61
pbdt1997 0:97879c8efe59 62 float readEnc2(){
pbdt1997 0:97879c8efe59 63 //Encoder2 = LOW
pbdt1997 0:97879c8efe59 64 csEnc2 = 0;
pbdt1997 0:97879c8efe59 65 char *arrAdd = &encArr[0];
pbdt1997 0:97879c8efe59 66 int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2);
pbdt1997 0:97879c8efe59 67 encData = encArr[0] * 256 + encArr[1];
pbdt1997 0:97879c8efe59 68 csEnc2 = 1;
pbdt1997 0:97879c8efe59 69 return ConvertAngle(encData/2);
pbdt1997 0:97879c8efe59 70 }
pbdt1997 0:97879c8efe59 71
pbdt1997 0:97879c8efe59 72 //Main
pbdt1997 0:97879c8efe59 73
pbdt1997 0:97879c8efe59 74 int main() {
pbdt1997 0:97879c8efe59 75 pc.baud(9600);
pbdt1997 0:97879c8efe59 76 RX_INT();
pbdt1997 0:97879c8efe59 77 pc.attach(&RX_INT, Serial::RxIrq);
pbdt1997 0:97879c8efe59 78
pbdt1997 0:97879c8efe59 79 csMstr = 1;
pbdt1997 0:97879c8efe59 80 csEnc1 = 1;
pbdt1997 0:97879c8efe59 81 csEnc2 = 1;
pbdt1997 0:97879c8efe59 82
pbdt1997 0:97879c8efe59 83 master.format(8,3);
pbdt1997 0:97879c8efe59 84 master.frequency(1000000);
pbdt1997 0:97879c8efe59 85 mstrEnc1.format(14, 3);
pbdt1997 0:97879c8efe59 86 mstrEnc1.frequency(500000);
pbdt1997 0:97879c8efe59 87 mstrEnc2.format(14, 3);
pbdt1997 0:97879c8efe59 88 mstrEnc2.frequency(500000);
pbdt1997 0:97879c8efe59 89
pbdt1997 0:97879c8efe59 90
pbdt1997 0:97879c8efe59 91 //Program Loop
pbdt1997 0:97879c8efe59 92 while(true){
pbdt1997 0:97879c8efe59 93 if(getData == 1){
pbdt1997 0:97879c8efe59 94 for(char i = 0; i < arraySize; i++){
pbdt1997 0:97879c8efe59 95 //printf("array[%d] = %d\n", i,array[i]);
pbdt1997 0:97879c8efe59 96 // csMstr = 0;
pbdt1997 0:97879c8efe59 97 // master.write(array[i]);
pbdt1997 0:97879c8efe59 98 // int masterData = master.write(0x00);
pbdt1997 0:97879c8efe59 99 // printf("masterData = %d\n", masterData);
pbdt1997 0:97879c8efe59 100 pc.putc(array[i]);
pbdt1997 0:97879c8efe59 101 }
pbdt1997 0:97879c8efe59 102 if(array[2] == 122){ //receive 'z'
pbdt1997 0:97879c8efe59 103 start = true;
pbdt1997 0:97879c8efe59 104 csMstr = 0;
pbdt1997 0:97879c8efe59 105 master.write(array[3]);
pbdt1997 0:97879c8efe59 106 int masterData = master.write(0x00);
pbdt1997 0:97879c8efe59 107 //printf("masterData = %d\n", masterData);
pbdt1997 0:97879c8efe59 108 //state = 0;
pbdt1997 0:97879c8efe59 109 csMstr = 1;
pbdt1997 0:97879c8efe59 110 pc.putc(masterData);
pbdt1997 0:97879c8efe59 111 pc.putc('A');
pbdt1997 0:97879c8efe59 112
pbdt1997 0:97879c8efe59 113 // wait(1);
pbdt1997 0:97879c8efe59 114 }
pbdt1997 0:97879c8efe59 115
pbdt1997 0:97879c8efe59 116 getData = 0;
pbdt1997 0:97879c8efe59 117 }
pbdt1997 0:97879c8efe59 118 if(start == true){
pbdt1997 0:97879c8efe59 119 float encData1 = readEnc1();
pbdt1997 0:97879c8efe59 120 float encData2 = readEnc2();
pbdt1997 0:97879c8efe59 121 printf("Encoder1 = %.2f ", encData1);
pbdt1997 0:97879c8efe59 122 printf("Encoder2 = %.2f \n\r", encData2);
pbdt1997 0:97879c8efe59 123 }
pbdt1997 0:97879c8efe59 124
pbdt1997 0:97879c8efe59 125 }
pbdt1997 0:97879c8efe59 126
pbdt1997 0:97879c8efe59 127
pbdt1997 0:97879c8efe59 128
pbdt1997 0:97879c8efe59 129
pbdt1997 0:97879c8efe59 130 }