Robotics Studio
/
readEncoder
sss
main.cpp@1:d935f9ed6be5, 2019-04-20 (annotated)
- Committer:
- pbdt1997
- Date:
- Sat Apr 20 09:56:01 2019 +0000
- Revision:
- 1:d935f9ed6be5
- Parent:
- 0:eac841605c08
read encoder;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pbdt1997 | 0:eac841605c08 | 1 | #include "mbed.h" |
pbdt1997 | 0:eac841605c08 | 2 | #include <Map.hpp> |
pbdt1997 | 0:eac841605c08 | 3 | |
pbdt1997 | 0:eac841605c08 | 4 | //Variable Declarations |
pbdt1997 | 0:eac841605c08 | 5 | |
pbdt1997 | 0:eac841605c08 | 6 | Serial pc(USBTX, USBRX); |
pbdt1997 | 1:d935f9ed6be5 | 7 | SPI mstrEnc1(PB_15, PB_14, PB_13); //SPI2_MOSI, SPI2_MISO, SPI2_SCLK |
pbdt1997 | 1:d935f9ed6be5 | 8 | SPI mstrEnc2(PC_12, PC_11, PC_10); //SPI3_MOSI, SPI3_MISO, SPI3_SCLK |
pbdt1997 | 0:eac841605c08 | 9 | |
pbdt1997 | 1:d935f9ed6be5 | 10 | DigitalOut csEnc1(PB_12); //SPI2_SSEL Encoder 1 |
pbdt1997 | 1:d935f9ed6be5 | 11 | DigitalOut csEnc2(PA_4); //SPI3_SSEL Encoder 2 |
pbdt1997 | 0:eac841605c08 | 12 | |
pbdt1997 | 0:eac841605c08 | 13 | char rx_in = 0, rx_out = 0; |
pbdt1997 | 0:eac841605c08 | 14 | |
pbdt1997 | 0:eac841605c08 | 15 | char dataSize = 5; |
pbdt1997 | 0:eac841605c08 | 16 | char array[5]; |
pbdt1997 | 0:eac841605c08 | 17 | char arraySize = sizeof(array); |
pbdt1997 | 0:eac841605c08 | 18 | char getData = 0; |
pbdt1997 | 0:eac841605c08 | 19 | char checkData = 0; |
pbdt1997 | 0:eac841605c08 | 20 | char encArr[2]= {255,255}; |
pbdt1997 | 0:eac841605c08 | 21 | char encLength = 2; |
pbdt1997 | 0:eac841605c08 | 22 | |
pbdt1997 | 0:eac841605c08 | 23 | ///////////////////////////// |
pbdt1997 | 0:eac841605c08 | 24 | |
pbdt1997 | 0:eac841605c08 | 25 | //Functions |
pbdt1997 | 0:eac841605c08 | 26 | |
pbdt1997 | 0:eac841605c08 | 27 | void RX(int data){ |
pbdt1997 | 0:eac841605c08 | 28 | array[checkData] = data; |
pbdt1997 | 0:eac841605c08 | 29 | checkData++; |
pbdt1997 | 0:eac841605c08 | 30 | if(checkData == arraySize){ |
pbdt1997 | 0:eac841605c08 | 31 | getData = 1; |
pbdt1997 | 0:eac841605c08 | 32 | checkData = 0; |
pbdt1997 | 0:eac841605c08 | 33 | } |
pbdt1997 | 0:eac841605c08 | 34 | } |
pbdt1997 | 0:eac841605c08 | 35 | |
pbdt1997 | 0:eac841605c08 | 36 | void RX_INT(){ |
pbdt1997 | 0:eac841605c08 | 37 | int data = pc.getc(); |
pbdt1997 | 0:eac841605c08 | 38 | RX(data); |
pbdt1997 | 0:eac841605c08 | 39 | } |
pbdt1997 | 0:eac841605c08 | 40 | |
pbdt1997 | 0:eac841605c08 | 41 | float ConvertAngle(int encData){ |
pbdt1997 | 0:eac841605c08 | 42 | Map map(0, 16383, 0, 359); |
pbdt1997 | 0:eac841605c08 | 43 | float angle = map.Calculate(encData); |
pbdt1997 | 0:eac841605c08 | 44 | return angle; |
pbdt1997 | 0:eac841605c08 | 45 | } |
pbdt1997 | 0:eac841605c08 | 46 | |
pbdt1997 | 1:d935f9ed6be5 | 47 | //read encoder of joint 1 |
pbdt1997 | 1:d935f9ed6be5 | 48 | float readEnc1(){ |
pbdt1997 | 1:d935f9ed6be5 | 49 | csEnc1 = 0; |
pbdt1997 | 0:eac841605c08 | 50 | char *arrAdd = &encArr[0]; |
pbdt1997 | 1:d935f9ed6be5 | 51 | int encData = mstrEnc1.write(arrAdd, 2, arrAdd, 2); |
pbdt1997 | 0:eac841605c08 | 52 | encData = encArr[0] * 256 + encArr[1]; |
pbdt1997 | 1:d935f9ed6be5 | 53 | csEnc1 = 1; |
pbdt1997 | 1:d935f9ed6be5 | 54 | return ConvertAngle(encData/2); |
pbdt1997 | 1:d935f9ed6be5 | 55 | } |
pbdt1997 | 1:d935f9ed6be5 | 56 | |
pbdt1997 | 1:d935f9ed6be5 | 57 | //read encoder of joint 2 |
pbdt1997 | 1:d935f9ed6be5 | 58 | float readEnc2(){ |
pbdt1997 | 1:d935f9ed6be5 | 59 | csEnc2 = 0; |
pbdt1997 | 1:d935f9ed6be5 | 60 | char *arrAdd = &encArr[0]; |
pbdt1997 | 1:d935f9ed6be5 | 61 | int encData = mstrEnc2.write(arrAdd, 2, arrAdd, 2); |
pbdt1997 | 1:d935f9ed6be5 | 62 | encData = encArr[0] * 256 + encArr[1]; |
pbdt1997 | 1:d935f9ed6be5 | 63 | csEnc2 = 1; |
pbdt1997 | 0:eac841605c08 | 64 | return ConvertAngle(encData/2); |
pbdt1997 | 0:eac841605c08 | 65 | } |
pbdt1997 | 0:eac841605c08 | 66 | |
pbdt1997 | 0:eac841605c08 | 67 | |
pbdt1997 | 0:eac841605c08 | 68 | void RX_INT(); |
pbdt1997 | 0:eac841605c08 | 69 | |
pbdt1997 | 0:eac841605c08 | 70 | //Main |
pbdt1997 | 0:eac841605c08 | 71 | |
pbdt1997 | 0:eac841605c08 | 72 | int main() { |
pbdt1997 | 1:d935f9ed6be5 | 73 | pc.baud(9600); |
pbdt1997 | 0:eac841605c08 | 74 | pc.attach(&RX_INT, Serial::RxIrq); |
pbdt1997 | 0:eac841605c08 | 75 | |
pbdt1997 | 1:d935f9ed6be5 | 76 | csEnc1 = 1; |
pbdt1997 | 1:d935f9ed6be5 | 77 | csEnc2 = 1; |
pbdt1997 | 0:eac841605c08 | 78 | |
pbdt1997 | 1:d935f9ed6be5 | 79 | mstrEnc1.format(14, 3); |
pbdt1997 | 1:d935f9ed6be5 | 80 | mstrEnc1.frequency(500000); |
pbdt1997 | 1:d935f9ed6be5 | 81 | mstrEnc2.format(14, 3); |
pbdt1997 | 1:d935f9ed6be5 | 82 | mstrEnc2.frequency(500000); |
pbdt1997 | 0:eac841605c08 | 83 | |
pbdt1997 | 0:eac841605c08 | 84 | |
pbdt1997 | 0:eac841605c08 | 85 | //Program Loop |
pbdt1997 | 0:eac841605c08 | 86 | while(true){ |
pbdt1997 | 0:eac841605c08 | 87 | //if(getData == 1){ |
pbdt1997 | 0:eac841605c08 | 88 | // for(char i = 0; i < arraySize; i++){ |
pbdt1997 | 0:eac841605c08 | 89 | // printf("array[%d] = %d\n", i,array[i]); |
pbdt1997 | 0:eac841605c08 | 90 | // pc.putc(array[i]); |
pbdt1997 | 0:eac841605c08 | 91 | //} |
pbdt1997 | 0:eac841605c08 | 92 | // pc.putc('A'); |
pbdt1997 | 0:eac841605c08 | 93 | // getData = 0; |
pbdt1997 | 0:eac841605c08 | 94 | //} |
pbdt1997 | 1:d935f9ed6be5 | 95 | float encdat1 = readEnc1(); |
pbdt1997 | 1:d935f9ed6be5 | 96 | float encdat2 = readEnc2(); |
pbdt1997 | 1:d935f9ed6be5 | 97 | printf("enc1 = %.2f, enc2 = %.2f \n\r", encdat1, encdat2); |
pbdt1997 | 0:eac841605c08 | 98 | } |
pbdt1997 | 0:eac841605c08 | 99 | |
pbdt1997 | 0:eac841605c08 | 100 | |
pbdt1997 | 0:eac841605c08 | 101 | |
pbdt1997 | 0:eac841605c08 | 102 | |
pbdt1997 | 0:eac841605c08 | 103 | } |