Robocon_IPS / Mbed OS RosSerial_Encoder

4 encoders, gy25, publisher

Dependencies:   QEI ros_lib_kinetic

main.cpp@0:62729afe5606, 2017-08-10 (annotated)

Committer:
iskenny4
Date:
Thu Aug 10 09:07:54 2017 +0000
Revision:
0:62729afe5606
Child:
1:ea4150fafa3b
for F446RE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
iskenny4 0:62729afe5606 1 /*
iskenny4 0:62729afe5606 2 * rosserial Publisher for encoder
iskenny4 0:62729afe5606 3 * Publish: distance(cm)
iskenny4 0:62729afe5606 4 */
iskenny4 0:62729afe5606 5
iskenny4 0:62729afe5606 6 #include"mbed.h"
iskenny4 0:62729afe5606 7 #include <ros.h>
iskenny4 0:62729afe5606 8 #include <std_msgs/Float32.h>
iskenny4 0:62729afe5606 9 #include "QEI.h"
iskenny4 0:62729afe5606 10
iskenny4 0:62729afe5606 11 // GYO
iskenny4 0:62729afe5606 12 Serial gy25(PA_9, PA_10, 115200);
iskenny4 0:62729afe5606 13 float g[3];
iskenny4 0:62729afe5606 14 char rxC[8];
iskenny4 0:62729afe5606 15 char ctBuffer[128];
iskenny4 0:62729afe5606 16
iskenny4 0:62729afe5606 17 void rxIRQ(){
iskenny4 0:62729afe5606 18 // get the Byte0: 0xAA Preamble Flags
iskenny4 0:62729afe5606 19 rxC[0] = gy25.getc();
iskenny4 0:62729afe5606 20 // fetch the 8-byte data when 0xAA is found
iskenny4 0:62729afe5606 21 if (rxC[0] == 0xAA) {
iskenny4 0:62729afe5606 22 for (int i = 1; i < 8; i++) {
iskenny4 0:62729afe5606 23 rxC[i] = gy25.getc();
iskenny4 0:62729afe5606 24 }
iskenny4 0:62729afe5606 25 // if the last byte is 0x55(Frame end flag), starts the calculation
iskenny4 0:62729afe5606 26 if (rxC[7] == 0x55) {
iskenny4 0:62729afe5606 27 for (int i = 0, j =1; i < 3; i++, j+=2) {
iskenny4 0:62729afe5606 28 //Angle = ( (HIGH << 8) | LOW ) / 100
iskenny4 0:62729afe5606 29 g[i] = (float)(rxC[j]<<8 | rxC[j+1])/100;
iskenny4 0:62729afe5606 30 // adjustment
iskenny4 0:62729afe5606 31 if (g[i] > 475)
iskenny4 0:62729afe5606 32 g[i] = g[i] - 476 + 180;
iskenny4 0:62729afe5606 33 }
iskenny4 0:62729afe5606 34 }
iskenny4 0:62729afe5606 35 }
iskenny4 0:62729afe5606 36 }
iskenny4 0:62729afe5606 37
iskenny4 0:62729afe5606 38 // ENCODER
iskenny4 0:62729afe5606 39 #define N 1000
iskenny4 0:62729afe5606 40 #define C 10 // circumference(cm)
iskenny4 0:62729afe5606 41 QEI En_X1(PC_10, PC_11, PC_12, N, QEI::X4_ENCODING); // A, B, Z, pulses/revolution, mode
iskenny4 0:62729afe5606 42 QEI En_X2(PC_1, PC_2, PC_3, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 43 QEI En_Y1(PB_3, PB_4, PB_5, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 44 QEI En_Y2(PA_6, PA_7, PA_8, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 45
iskenny4 0:62729afe5606 46 // instantiate the node handle
iskenny4 0:62729afe5606 47 ros::NodeHandle nh;
iskenny4 0:62729afe5606 48
iskenny4 0:62729afe5606 49 // instantiate the publisher
iskenny4 0:62729afe5606 50 std_msgs::Float32 X1, X2, Y1, Y2, G1, G2, G3;
iskenny4 0:62729afe5606 51 ros::Publisher pub_X1("X1", &X1);
iskenny4 0:62729afe5606 52 ros::Publisher pub_X2("X2", &X2);
iskenny4 0:62729afe5606 53 ros::Publisher pub_Y1("Y1", &Y1);
iskenny4 0:62729afe5606 54 ros::Publisher pub_Y2("Y2", &Y2);
iskenny4 0:62729afe5606 55 ros::Publisher pub_G1("G1", &G1);
iskenny4 0:62729afe5606 56 ros::Publisher pub_G2("G2", &G2);
iskenny4 0:62729afe5606 57 ros::Publisher pub_G3("G3", &G3);
iskenny4 0:62729afe5606 58
iskenny4 0:62729afe5606 59 // initializing node
iskenny4 0:62729afe5606 60 void nodeInit() {
iskenny4 0:62729afe5606 61 nh.getHardware()->setBaud(115200);
iskenny4 0:62729afe5606 62 nh.initNode();
iskenny4 0:62729afe5606 63
iskenny4 0:62729afe5606 64 nh.advertise(pub_X1);
iskenny4 0:62729afe5606 65 nh.advertise(pub_X2);
iskenny4 0:62729afe5606 66 nh.advertise(pub_Y1);
iskenny4 0:62729afe5606 67 nh.advertise(pub_Y2);
iskenny4 0:62729afe5606 68 nh.advertise(pub_G1);
iskenny4 0:62729afe5606 69 nh.advertise(pub_G3);
iskenny4 0:62729afe5606 70 nh.advertise(pub_G2);
iskenny4 0:62729afe5606 71 }
iskenny4 0:62729afe5606 72
iskenny4 0:62729afe5606 73 // BUTTON for reset
iskenny4 0:62729afe5606 74 InterruptIn button(USER_BUTTON);
iskenny4 0:62729afe5606 75
iskenny4 0:62729afe5606 76 void pressed() {
iskenny4 0:62729afe5606 77 gy25.putc(0xA5);
iskenny4 0:62729afe5606 78 gy25.putc(0x55);
iskenny4 0:62729afe5606 79 gy25.putc(0xA5);
iskenny4 0:62729afe5606 80 gy25.putc(0x52);
iskenny4 0:62729afe5606 81 En_X1.reset();
iskenny4 0:62729afe5606 82 En_X2.reset();
iskenny4 0:62729afe5606 83 En_Y1.reset();
iskenny4 0:62729afe5606 84 En_Y2.reset();
iskenny4 0:62729afe5606 85 }
iskenny4 0:62729afe5606 86
iskenny4 0:62729afe5606 87 int main() {
iskenny4 0:62729afe5606 88 nodeInit();
iskenny4 0:62729afe5606 89
iskenny4 0:62729afe5606 90 // gyo
iskenny4 0:62729afe5606 91 gy25.format(8,SerialBase::None,1);
iskenny4 0:62729afe5606 92 gy25.attach(rxIRQ, Serial::RxIrq);
iskenny4 0:62729afe5606 93 // reset button
iskenny4 0:62729afe5606 94 button.fall(&pressed);
iskenny4 0:62729afe5606 95
iskenny4 0:62729afe5606 96 while (1) {
iskenny4 0:62729afe5606 97 X1.data = (float)En_X1.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 98 Y1.data = (float)En_Y1.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 99 X2.data = (float)En_X2.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 100 Y2.data = (float)En_Y2.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 101 G1.data = g[0];
iskenny4 0:62729afe5606 102 G2.data = g[1];
iskenny4 0:62729afe5606 103 G3.data = g[2];
iskenny4 0:62729afe5606 104 pub_X1.publish( &X1 );
iskenny4 0:62729afe5606 105 pub_Y1.publish( &Y1 );
iskenny4 0:62729afe5606 106 pub_X2.publish( &X2 );
iskenny4 0:62729afe5606 107 pub_Y2.publish( &Y2 );
iskenny4 0:62729afe5606 108 pub_G1.publish( &G1 );
iskenny4 0:62729afe5606 109 pub_G2.publish (&G2 );
iskenny4 0:62729afe5606 110 pub_G3.publish( &G3 );
iskenny4 0:62729afe5606 111 nh.spinOnce();
iskenny4 0:62729afe5606 112 wait_ms(50); // 20hz
iskenny4 0:62729afe5606 113 }
iskenny4 0:62729afe5606 114 }