sss

Dependencies:   mbed Map

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?

UserRevisionLine numberNew 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 }