Robocon_IPS / Mbed OS RosSerial_Encoder

4 encoders, gy25, publisher

Dependencies:   QEI ros_lib_kinetic

main.cpp@2:cec0c1ff1c30, 2017-08-14 (annotated)

Committer:
iskenny4
Date:
Mon Aug 14 10:04:15 2017 +0000
Revision:
2:cec0c1ff1c30
Parent:
1:ea4150fafa3b 
+if

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 1:ea4150fafa3b 4 * Subscribe: --once
iskenny4 0:62729afe5606 5 */
iskenny4 0:62729afe5606 6
iskenny4 0:62729afe5606 7 #include"mbed.h"
iskenny4 0:62729afe5606 8 #include <ros.h>
iskenny4 0:62729afe5606 9 #include <std_msgs/Float32.h>
iskenny4 1:ea4150fafa3b 10 #include <std_msgs/Empty.h>
iskenny4 0:62729afe5606 11 #include "QEI.h"
iskenny4 0:62729afe5606 12
iskenny4 1:ea4150fafa3b 13 using namespace std_msgs;
iskenny4 1:ea4150fafa3b 14 using namespace ros;
iskenny4 1:ea4150fafa3b 15
iskenny4 1:ea4150fafa3b 16 DigitalOut myled(LED1);
iskenny4 1:ea4150fafa3b 17
iskenny4 1:ea4150fafa3b 18 #define BAUD 115200
iskenny4 1:ea4150fafa3b 19
iskenny4 0:62729afe5606 20 // GYO
iskenny4 0:62729afe5606 21 Serial gy25(PA_9, PA_10, 115200);
iskenny4 0:62729afe5606 22 float g[3];
iskenny4 0:62729afe5606 23 char rxC[8];
iskenny4 0:62729afe5606 24 char ctBuffer[128];
iskenny4 0:62729afe5606 25
iskenny4 0:62729afe5606 26 void rxIRQ(){
iskenny4 0:62729afe5606 27 // get the Byte0: 0xAA Preamble Flags
iskenny4 0:62729afe5606 28 rxC[0] = gy25.getc();
iskenny4 0:62729afe5606 29 // fetch the 8-byte data when 0xAA is found
iskenny4 0:62729afe5606 30 if (rxC[0] == 0xAA) {
iskenny4 0:62729afe5606 31 for (int i = 1; i < 8; i++) {
iskenny4 0:62729afe5606 32 rxC[i] = gy25.getc();
iskenny4 0:62729afe5606 33 }
iskenny4 0:62729afe5606 34 // if the last byte is 0x55(Frame end flag), starts the calculation
iskenny4 0:62729afe5606 35 if (rxC[7] == 0x55) {
iskenny4 0:62729afe5606 36 for (int i = 0, j =1; i < 3; i++, j+=2) {
iskenny4 0:62729afe5606 37 //Angle = ( (HIGH << 8) | LOW ) / 100
iskenny4 0:62729afe5606 38 g[i] = (float)(rxC[j]<<8 | rxC[j+1])/100;
iskenny4 0:62729afe5606 39 // adjustment
iskenny4 0:62729afe5606 40 if (g[i] > 475)
iskenny4 0:62729afe5606 41 g[i] = g[i] - 476 + 180;
iskenny4 0:62729afe5606 42 }
iskenny4 0:62729afe5606 43 }
iskenny4 0:62729afe5606 44 }
iskenny4 0:62729afe5606 45 }
iskenny4 0:62729afe5606 46
iskenny4 0:62729afe5606 47 // ENCODER
iskenny4 0:62729afe5606 48 #define N 1000
iskenny4 0:62729afe5606 49 #define C 10 // circumference(cm)
iskenny4 0:62729afe5606 50 QEI En_X1(PC_10, PC_11, PC_12, N, QEI::X4_ENCODING); // A, B, Z, pulses/revolution, mode
iskenny4 0:62729afe5606 51 QEI En_X2(PC_1, PC_2, PC_3, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 52 QEI En_Y1(PB_3, PB_4, PB_5, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 53 QEI En_Y2(PA_6, PA_7, PA_8, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 54
iskenny4 0:62729afe5606 55 // instantiate the node handle
iskenny4 0:62729afe5606 56 ros::NodeHandle nh;
iskenny4 0:62729afe5606 57
iskenny4 0:62729afe5606 58 // instantiate the publisher
iskenny4 1:ea4150fafa3b 59 Float32 X1, X2, Y1, Y2, G1, G2, G3;
iskenny4 1:ea4150fafa3b 60 Publisher pub_X1("X1", &X1);
iskenny4 1:ea4150fafa3b 61 Publisher pub_X2("X2", &X2);
iskenny4 1:ea4150fafa3b 62 Publisher pub_Y1("Y1", &Y1);
iskenny4 1:ea4150fafa3b 63 Publisher pub_Y2("Y2", &Y2);
iskenny4 1:ea4150fafa3b 64 Publisher pub_G1("G1", &G1);
iskenny4 1:ea4150fafa3b 65 Publisher pub_G2("G2", &G2);
iskenny4 1:ea4150fafa3b 66 Publisher pub_G3("G3", &G3);
iskenny4 0:62729afe5606 67
iskenny4 0:62729afe5606 68 // BUTTON for reset
iskenny4 0:62729afe5606 69 InterruptIn button(USER_BUTTON);
iskenny4 0:62729afe5606 70
iskenny4 0:62729afe5606 71 void pressed() {
iskenny4 0:62729afe5606 72 gy25.putc(0xA5);
iskenny4 0:62729afe5606 73 gy25.putc(0x55);
iskenny4 0:62729afe5606 74 gy25.putc(0xA5);
iskenny4 0:62729afe5606 75 gy25.putc(0x52);
iskenny4 0:62729afe5606 76 En_X1.reset();
iskenny4 0:62729afe5606 77 En_X2.reset();
iskenny4 0:62729afe5606 78 En_Y1.reset();
iskenny4 0:62729afe5606 79 En_Y2.reset();
iskenny4 0:62729afe5606 80 }
iskenny4 0:62729afe5606 81
iskenny4 1:ea4150fafa3b 82 // for reset
iskenny4 1:ea4150fafa3b 83 void messageCb(const Empty& toggle_msg){
iskenny4 1:ea4150fafa3b 84 pressed();
iskenny4 1:ea4150fafa3b 85 myled = !myled; // blink the led
iskenny4 1:ea4150fafa3b 86 }
iskenny4 1:ea4150fafa3b 87
iskenny4 1:ea4150fafa3b 88 Subscriber<Empty> sub("toggle_led", &messageCb);
iskenny4 1:ea4150fafa3b 89
iskenny4 1:ea4150fafa3b 90 // initializing node
iskenny4 1:ea4150fafa3b 91 void nodeInit() {
iskenny4 1:ea4150fafa3b 92 nh.getHardware()->setBaud(BAUD);
iskenny4 1:ea4150fafa3b 93 nh.initNode();
iskenny4 1:ea4150fafa3b 94
iskenny4 1:ea4150fafa3b 95 nh.subscribe(sub);
iskenny4 1:ea4150fafa3b 96
iskenny4 1:ea4150fafa3b 97 nh.advertise(pub_X1);
iskenny4 1:ea4150fafa3b 98 nh.advertise(pub_X2);
iskenny4 1:ea4150fafa3b 99 nh.advertise(pub_Y1);
iskenny4 1:ea4150fafa3b 100 nh.advertise(pub_Y2);
iskenny4 1:ea4150fafa3b 101 nh.advertise(pub_G1);
iskenny4 1:ea4150fafa3b 102 nh.advertise(pub_G3);
iskenny4 1:ea4150fafa3b 103 nh.advertise(pub_G2);
iskenny4 1:ea4150fafa3b 104 }
iskenny4 1:ea4150fafa3b 105
iskenny4 2:cec0c1ff1c30 106 float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0;
iskenny4 2:cec0c1ff1c30 107
iskenny4 0:62729afe5606 108 int main() {
iskenny4 0:62729afe5606 109 nodeInit();
iskenny4 0:62729afe5606 110
iskenny4 0:62729afe5606 111 // gyo
iskenny4 0:62729afe5606 112 gy25.format(8,SerialBase::None,1);
iskenny4 0:62729afe5606 113 gy25.attach(rxIRQ, Serial::RxIrq);
iskenny4 0:62729afe5606 114 // reset button
iskenny4 0:62729afe5606 115 button.fall(&pressed);
iskenny4 0:62729afe5606 116
iskenny4 0:62729afe5606 117 while (1) {
iskenny4 0:62729afe5606 118 X1.data = (float)En_X1.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 119 Y1.data = (float)En_Y1.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 120 X2.data = (float)En_X2.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 121 Y2.data = (float)En_Y2.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 122 G1.data = g[0];
iskenny4 0:62729afe5606 123 G2.data = g[1];
iskenny4 0:62729afe5606 124 G3.data = g[2];
iskenny4 2:cec0c1ff1c30 125 if(pX1 != X1.data){
iskenny4 2:cec0c1ff1c30 126 pX1 = X1.data;
iskenny4 2:cec0c1ff1c30 127 pub_X1.publish( &X1 );
iskenny4 2:cec0c1ff1c30 128 }
iskenny4 2:cec0c1ff1c30 129 if(pX2 != X2.data){
iskenny4 2:cec0c1ff1c30 130 pX2 = X2.data;
iskenny4 2:cec0c1ff1c30 131 pub_X2.publish( &X2 );
iskenny4 2:cec0c1ff1c30 132 }
iskenny4 2:cec0c1ff1c30 133 if(pY1 != Y1.data){
iskenny4 2:cec0c1ff1c30 134 pY1 = Y1.data;
iskenny4 2:cec0c1ff1c30 135 pub_Y1.publish( &Y1 );
iskenny4 2:cec0c1ff1c30 136 }
iskenny4 2:cec0c1ff1c30 137 if(pY2 != Y2.data){
iskenny4 2:cec0c1ff1c30 138 pY2 = Y2.data;
iskenny4 2:cec0c1ff1c30 139 pub_Y2.publish( &Y2 );
iskenny4 2:cec0c1ff1c30 140 }
iskenny4 2:cec0c1ff1c30 141 if(pG1 != G1.data){
iskenny4 2:cec0c1ff1c30 142 pG1 = G1.data;
iskenny4 2:cec0c1ff1c30 143 pub_G1.publish( &G1 );
iskenny4 2:cec0c1ff1c30 144 }
iskenny4 2:cec0c1ff1c30 145 if(pG2 != G2.data){
iskenny4 2:cec0c1ff1c30 146 pG2 = G2.data;
iskenny4 2:cec0c1ff1c30 147 pub_G2.publish( &G2 );
iskenny4 2:cec0c1ff1c30 148 }
iskenny4 2:cec0c1ff1c30 149 if(pG3 != G3.data){
iskenny4 2:cec0c1ff1c30 150 pG3 = G3.data;
iskenny4 2:cec0c1ff1c30 151 pub_G3.publish( &G3 );
iskenny4 2:cec0c1ff1c30 152 }
iskenny4 0:62729afe5606 153 nh.spinOnce();
iskenny4 0:62729afe5606 154 wait_ms(50); // 20hz
iskenny4 0:62729afe5606 155 }
iskenny4 0:62729afe5606 156 }