Robocon_IPS / Mbed OS RosSerial_Encoder_final

encoder final + coordinate

Dependencies:   QEI ros_lib_kinetic

Fork of RosSerial_Encoder by Robocon_IPS

main.cpp@3:0de505949008, 2018-01-12 (annotated)

Committer:
cc061495
Date:
Fri Jan 12 06:30:54 2018 +0000
Revision:
3:0de505949008
Parent:
2:af928306ca36 
final version

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"
cc061495 3:0de505949008 12 #include "math.h"
iskenny4 0:62729afe5606 13
iskenny4 1:ea4150fafa3b 14 using namespace std_msgs;
iskenny4 1:ea4150fafa3b 15 using namespace ros;
iskenny4 1:ea4150fafa3b 16
iskenny4 1:ea4150fafa3b 17 DigitalOut myled(LED1);
iskenny4 1:ea4150fafa3b 18
iskenny4 1:ea4150fafa3b 19 #define BAUD 115200
cc061495 3:0de505949008 20 #define M_PI 3.14159265358979323846 /* pi */
iskenny4 1:ea4150fafa3b 21
iskenny4 0:62729afe5606 22 // GYO
iskenny4 0:62729afe5606 23 Serial gy25(PA_9, PA_10, 115200);
iskenny4 0:62729afe5606 24 float g[3];
iskenny4 0:62729afe5606 25 char rxC[8];
iskenny4 0:62729afe5606 26 char ctBuffer[128];
iskenny4 0:62729afe5606 27
cc061495 3:0de505949008 28
cc061495 3:0de505949008 29 float pX1 = 0, pX2 = 0, pY1 = 0, pY2 = 0, pG1 = 0, pG2 = 0, pG3 = 0, pXaxis = 0, pYaxis = 0;
cc061495 3:0de505949008 30 float g0 = 0, Xn = 0, Yn = 0, Gn = 0, pX = 0, pY = 0, pg0 = 0;
cc061495 3:0de505949008 31 bool clockwise = false;
cc061495 3:0de505949008 32
iskenny4 0:62729afe5606 33 void rxIRQ(){
iskenny4 0:62729afe5606 34 // get the Byte0: 0xAA Preamble Flags
iskenny4 0:62729afe5606 35 rxC[0] = gy25.getc();
iskenny4 0:62729afe5606 36 // fetch the 8-byte data when 0xAA is found
iskenny4 0:62729afe5606 37 if (rxC[0] == 0xAA) {
iskenny4 0:62729afe5606 38 for (int i = 1; i < 8; i++) {
iskenny4 0:62729afe5606 39 rxC[i] = gy25.getc();
iskenny4 0:62729afe5606 40 }
iskenny4 0:62729afe5606 41 // if the last byte is 0x55(Frame end flag), starts the calculation
iskenny4 0:62729afe5606 42 if (rxC[7] == 0x55) {
iskenny4 0:62729afe5606 43 for (int i = 0, j =1; i < 3; i++, j+=2) {
iskenny4 0:62729afe5606 44 //Angle = ( (HIGH << 8) | LOW ) / 100
iskenny4 0:62729afe5606 45 g[i] = (float)(rxC[j]<<8 | rxC[j+1])/100;
iskenny4 0:62729afe5606 46 // adjustment
iskenny4 0:62729afe5606 47 if (g[i] > 475)
iskenny4 0:62729afe5606 48 g[i] = g[i] - 476 + 180;
iskenny4 0:62729afe5606 49 }
iskenny4 0:62729afe5606 50 }
iskenny4 0:62729afe5606 51 }
iskenny4 0:62729afe5606 52 }
iskenny4 0:62729afe5606 53
iskenny4 0:62729afe5606 54 // ENCODER
iskenny4 0:62729afe5606 55 #define N 1000
cc061495 3:0de505949008 56 #define C 18 // circumference(cm)
iskenny4 0:62729afe5606 57 QEI En_X1(PC_10, PC_11, PC_12, N, QEI::X4_ENCODING); // A, B, Z, pulses/revolution, mode
iskenny4 0:62729afe5606 58 QEI En_X2(PC_1, PC_2, PC_3, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 59 QEI En_Y1(PB_3, PB_4, PB_5, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 60 QEI En_Y2(PA_6, PA_7, PA_8, N, QEI::X4_ENCODING);
iskenny4 0:62729afe5606 61
iskenny4 0:62729afe5606 62 // instantiate the node handle
iskenny4 0:62729afe5606 63 ros::NodeHandle nh;
iskenny4 0:62729afe5606 64
iskenny4 0:62729afe5606 65 // instantiate the publisher
cc061495 3:0de505949008 66 Float32 Xaxis, Yaxis, X1, X2, Y1, Y2, G1, G2, G3;
cc061495 3:0de505949008 67 Publisher pub_X("Xaxis", &Xaxis);
cc061495 3:0de505949008 68 Publisher pub_Y("Yaxis", &Yaxis);
iskenny4 1:ea4150fafa3b 69 Publisher pub_X1("X1", &X1);
cc061495 3:0de505949008 70 Publisher pub_Y1("Y1", &Y1);
iskenny4 1:ea4150fafa3b 71 Publisher pub_X2("X2", &X2);
iskenny4 1:ea4150fafa3b 72 Publisher pub_Y2("Y2", &Y2);
iskenny4 1:ea4150fafa3b 73 Publisher pub_G1("G1", &G1);
iskenny4 1:ea4150fafa3b 74 Publisher pub_G2("G2", &G2);
iskenny4 1:ea4150fafa3b 75 Publisher pub_G3("G3", &G3);
iskenny4 0:62729afe5606 76
iskenny4 0:62729afe5606 77 // BUTTON for reset
iskenny4 0:62729afe5606 78 InterruptIn button(USER_BUTTON);
iskenny4 0:62729afe5606 79
iskenny4 0:62729afe5606 80 void pressed() {
iskenny4 0:62729afe5606 81 gy25.putc(0xA5);
iskenny4 0:62729afe5606 82 gy25.putc(0x55);
iskenny4 0:62729afe5606 83 gy25.putc(0xA5);
iskenny4 0:62729afe5606 84 gy25.putc(0x52);
iskenny4 0:62729afe5606 85 En_X1.reset();
iskenny4 0:62729afe5606 86 En_X2.reset();
iskenny4 0:62729afe5606 87 En_Y1.reset();
iskenny4 0:62729afe5606 88 En_Y2.reset();
cc061495 3:0de505949008 89 Xn = 0;
cc061495 3:0de505949008 90 Yn = 0;
iskenny4 0:62729afe5606 91 }
iskenny4 0:62729afe5606 92
iskenny4 1:ea4150fafa3b 93 // for reset
iskenny4 1:ea4150fafa3b 94 void messageCb(const Empty& toggle_msg){
iskenny4 1:ea4150fafa3b 95 pressed();
iskenny4 1:ea4150fafa3b 96 myled = !myled; // blink the led
iskenny4 1:ea4150fafa3b 97 }
iskenny4 1:ea4150fafa3b 98
iskenny4 1:ea4150fafa3b 99 Subscriber<Empty> sub("toggle_led", &messageCb);
iskenny4 1:ea4150fafa3b 100
iskenny4 1:ea4150fafa3b 101 // initializing node
iskenny4 1:ea4150fafa3b 102 void nodeInit() {
iskenny4 1:ea4150fafa3b 103 nh.getHardware()->setBaud(BAUD);
iskenny4 1:ea4150fafa3b 104 nh.initNode();
iskenny4 1:ea4150fafa3b 105
iskenny4 1:ea4150fafa3b 106 nh.subscribe(sub);
iskenny4 1:ea4150fafa3b 107
cc061495 3:0de505949008 108 nh.advertise(pub_X);
cc061495 3:0de505949008 109 nh.advertise(pub_Y);
iskenny4 1:ea4150fafa3b 110 nh.advertise(pub_X1);
cc061495 3:0de505949008 111 nh.advertise(pub_Y1);
iskenny4 1:ea4150fafa3b 112 nh.advertise(pub_X2);
iskenny4 1:ea4150fafa3b 113 nh.advertise(pub_Y2);
iskenny4 1:ea4150fafa3b 114 nh.advertise(pub_G1);
cc061495 3:0de505949008 115 nh.advertise(pub_G2);
iskenny4 1:ea4150fafa3b 116 nh.advertise(pub_G3);
iskenny4 1:ea4150fafa3b 117 }
iskenny4 1:ea4150fafa3b 118
cc061495 3:0de505949008 119
cc061495 2:af928306ca36 120
iskenny4 0:62729afe5606 121 int main() {
iskenny4 0:62729afe5606 122 nodeInit();
iskenny4 0:62729afe5606 123
iskenny4 0:62729afe5606 124 // gyo
iskenny4 0:62729afe5606 125 gy25.format(8,SerialBase::None,1);
iskenny4 0:62729afe5606 126 gy25.attach(rxIRQ, Serial::RxIrq);
iskenny4 0:62729afe5606 127 // reset button
iskenny4 0:62729afe5606 128 button.fall(&pressed);
iskenny4 0:62729afe5606 129
iskenny4 0:62729afe5606 130 while (1) {
cc061495 3:0de505949008 131 X1.data = -(float)En_X1.getPulses()/(4*N) * C;
cc061495 3:0de505949008 132 Y1.data = -(float)En_Y2.getPulses()/(4*N) * C;
iskenny4 0:62729afe5606 133 X2.data = (float)En_X2.getPulses()/(4*N) * C;
cc061495 3:0de505949008 134 Y2.data = (float)En_Y1.getPulses()/(4*N) * C;
cc061495 3:0de505949008 135
cc061495 3:0de505949008 136 float dX = ((X1.data - pX1) + (X2.data - pX2)) / 2;
cc061495 3:0de505949008 137 float dY = ((Y1.data - pY1) + (Y2.data - pY2)) / 2;
cc061495 3:0de505949008 138
cc061495 3:0de505949008 139 if(pG1 < 45 && pG1 >= 0 && floor(g[0]) > 315){
cc061495 3:0de505949008 140 clockwise = true;
cc061495 3:0de505949008 141 }
cc061495 3:0de505949008 142 else if(pG1 > 315 && floor(g[0]) >= 0 && floor(g[0]) < 45){
cc061495 3:0de505949008 143 clockwise = false;
cc061495 3:0de505949008 144 }
cc061495 3:0de505949008 145
cc061495 3:0de505949008 146 if(clockwise){
cc061495 3:0de505949008 147 g0 = g[0];
cc061495 3:0de505949008 148 //g0 = g[0] - 360;
cc061495 3:0de505949008 149 }
cc061495 3:0de505949008 150 else{
cc061495 3:0de505949008 151 g0 = g[0];
cc061495 3:0de505949008 152 }
cc061495 2:af928306ca36 153
cc061495 3:0de505949008 154 //float arc = 15 * g0 / 180 * M_PI;
cc061495 3:0de505949008 155 //float Xa = X1.data - arc;
cc061495 3:0de505949008 156 //float Ya = Y1.data - arc;
cc061495 3:0de505949008 157 // float dX = X - pX;
cc061495 3:0de505949008 158 // float dY = Y - pY;
cc061495 3:0de505949008 159 float dg0 = g0 - pg0;
cc061495 3:0de505949008 160
cc061495 3:0de505949008 161 float x = (Xn + dX * cos(Gn + dg0 / 360 * M_PI) + dY * cos(Gn + M_PI/2 + dg0 / 360 * M_PI));
cc061495 3:0de505949008 162 float y = (Yn + dX * sin(Gn + dg0 /360 * M_PI) + dY * sin(Gn + M_PI/2 + dg0 /360 * M_PI));
cc061495 3:0de505949008 163 Gn = Gn + dg0 / 180 * M_PI;
cc061495 3:0de505949008 164 pg0 = g0;
cc061495 3:0de505949008 165 Xn = x;
cc061495 3:0de505949008 166 Yn = y;
cc061495 3:0de505949008 167 //x = (int)(x*10);
cc061495 3:0de505949008 168 //y = (int)(y*10);
cc061495 3:0de505949008 169 //Xaxis.data = (float) x;
cc061495 3:0de505949008 170 //Yaxis.data = (float) y;
cc061495 3:0de505949008 171 Xaxis.data = (int)x;
cc061495 3:0de505949008 172 Yaxis.data = (int)y;
cc061495 3:0de505949008 173 // pX = dX;
cc061495 3:0de505949008 174 // pY = dY;
cc061495 3:0de505949008 175 //Xaxis.data = (int) (Xa * cos(g0 / 180 * M_PI) - Ya * sin(g0 / 180 * M_PI));
cc061495 3:0de505949008 176 //Yaxis.data = (int) (Ya * cos(g0 / 180 * M_PI) + Xa * sin(g0 / 180 * M_PI));
cc061495 3:0de505949008 177
cc061495 3:0de505949008 178 pub_X.publish( &Xaxis );
cc061495 3:0de505949008 179 pub_Y.publish( &Yaxis );
cc061495 3:0de505949008 180
cc061495 3:0de505949008 181 G1.data = floor(g[0]);
cc061495 3:0de505949008 182 G2.data = dX;
cc061495 3:0de505949008 183 G3.data = dY;
cc061495 3:0de505949008 184
cc061495 3:0de505949008 185
cc061495 2:af928306ca36 186 if(pX1 != X1.data){
cc061495 2:af928306ca36 187 pX1 = X1.data;
cc061495 2:af928306ca36 188 pub_X1.publish( &X1 );
cc061495 2:af928306ca36 189 }
cc061495 2:af928306ca36 190 if(pX2 != X2.data){
cc061495 2:af928306ca36 191 pX2 = X2.data;
cc061495 2:af928306ca36 192 pub_X2.publish( &X2 );
cc061495 2:af928306ca36 193 }
cc061495 2:af928306ca36 194 if(pY1 != Y1.data){
cc061495 2:af928306ca36 195 pY1 = Y1.data;
cc061495 2:af928306ca36 196 pub_Y1.publish( &Y1 );
cc061495 2:af928306ca36 197 }
cc061495 2:af928306ca36 198 if(pY2 != Y2.data){
cc061495 2:af928306ca36 199 pY2 = Y2.data;
cc061495 2:af928306ca36 200 pub_Y2.publish( &Y2 );
cc061495 2:af928306ca36 201 }
cc061495 2:af928306ca36 202 if(pG1 != G1.data){
cc061495 2:af928306ca36 203 pG1 = G1.data;
cc061495 2:af928306ca36 204 pub_G1.publish( &G1 );
cc061495 2:af928306ca36 205 }
cc061495 2:af928306ca36 206 if(pG2 != G2.data){
cc061495 2:af928306ca36 207 pG2 = G2.data;
cc061495 2:af928306ca36 208 pub_G2.publish( &G2 );
cc061495 2:af928306ca36 209 }
cc061495 2:af928306ca36 210 if(pG3 != G3.data){
cc061495 2:af928306ca36 211 pG3 = G3.data;
cc061495 2:af928306ca36 212 pub_G3.publish( &G3 );
cc061495 2:af928306ca36 213 }
iskenny4 0:62729afe5606 214 nh.spinOnce();
iskenny4 0:62729afe5606 215 wait_ms(50); // 20hz
iskenny4 0:62729afe5606 216 }
iskenny4 0:62729afe5606 217 }