Sam Shum / ros_mbed_base_controller

Sam

ActionEncoder.txt@0:502b364c9f1d, 2018-10-21 (annotated)

Committer:
s0313045
Date:
Sun Oct 21 19:38:09 2018 +0000
Revision:
0:502b364c9f1d
main_stable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s0313045 0:502b364c9f1d 1 #include "mbed.h"
s0313045 0:502b364c9f1d 2 #include "millis.h"
s0313045 0:502b364c9f1d 3
s0313045 0:502b364c9f1d 4 /*
s0313045 0:502b364c9f1d 5 * ActionEncoder.cpp
s0313045 0:502b364c9f1d 6 *
s0313045 0:502b364c9f1d 7 * Created on: 7 Mar 2018
s0313045 0:502b364c9f1d 8 * Author: tung
s0313045 0:502b364c9f1d 9 */
s0313045 0:502b364c9f1d 10
s0313045 0:502b364c9f1d 11 #include "ActionEncoder.hpp"
s0313045 0:502b364c9f1d 12 #include "Timer.h"
s0313045 0:502b364c9f1d 13
s0313045 0:502b364c9f1d 14 Serial Action(D8,D2 ); // tx, rx
s0313045 0:502b364c9f1d 15 Serial pc(USBTX, USBRX);
s0313045 0:502b364c9f1d 16
s0313045 0:502b364c9f1d 17
s0313045 0:502b364c9f1d 18
s0313045 0:502b364c9f1d 19 union {
s0313045 0:502b364c9f1d 20 uint8_t data[24];
s0313045 0:502b364c9f1d 21 float val[6];
s0313045 0:502b364c9f1d 22 } posture;
s0313045 0:502b364c9f1d 23 int count=0;
s0313045 0:502b364c9f1d 24 int i=0;
s0313045 0:502b364c9f1d 25 int done=0;
s0313045 0:502b364c9f1d 26 float xangle=0;
s0313045 0:502b364c9f1d 27 float yangle=0;
s0313045 0:502b364c9f1d 28 float zangle=0;
s0313045 0:502b364c9f1d 29 float d_angle=0;
s0313045 0:502b364c9f1d 30 float pos_x=0;
s0313045 0:502b364c9f1d 31 float pos_y=0;
s0313045 0:502b364c9f1d 32 float angleSpeed=0;
s0313045 0:502b364c9f1d 33 float temp_zangle=0;
s0313045 0:502b364c9f1d 34 int LastRead=0;
s0313045 0:502b364c9f1d 35 bool newDataArrived=false;
s0313045 0:502b364c9f1d 36
s0313045 0:502b364c9f1d 37 char buffer[8];
s0313045 0:502b364c9f1d 38
s0313045 0:502b364c9f1d 39
s0313045 0:502b364c9f1d 40 void ActionEncoder_init()
s0313045 0:502b364c9f1d 41 {
s0313045 0:502b364c9f1d 42 count=0;
s0313045 0:502b364c9f1d 43 i=0;
s0313045 0:502b364c9f1d 44 done=0;
s0313045 0:502b364c9f1d 45 xangle=0;
s0313045 0:502b364c9f1d 46 yangle=0;
s0313045 0:502b364c9f1d 47 zangle=0;
s0313045 0:502b364c9f1d 48 d_angle=0;
s0313045 0:502b364c9f1d 49 pos_x=0;
s0313045 0:502b364c9f1d 50 pos_y=0;
s0313045 0:502b364c9f1d 51 angleSpeed=0;
s0313045 0:502b364c9f1d 52 temp_zangle=0;
s0313045 0:502b364c9f1d 53 LastRead=0;
s0313045 0:502b364c9f1d 54 newDataArrived=false;
s0313045 0:502b364c9f1d 55
s0313045 0:502b364c9f1d 56
s0313045 0:502b364c9f1d 57 }
s0313045 0:502b364c9f1d 58
s0313045 0:502b364c9f1d 59 void readEncoder(char c)
s0313045 0:502b364c9f1d 60 {
s0313045 0:502b364c9f1d 61
s0313045 0:502b364c9f1d 62
s0313045 0:502b364c9f1d 63 //sprintf(buffer,"%02X",c);
s0313045 0:502b364c9f1d 64 //sprintf(buffer,"%X",c);
s0313045 0:502b364c9f1d 65 //pc.printf(buffer);
s0313045 0:502b364c9f1d 66 //pc.printf("\r\n");
s0313045 0:502b364c9f1d 67
s0313045 0:502b364c9f1d 68 //sprintf(buffer,"%d",count);
s0313045 0:502b364c9f1d 69 //pc.printf(buffer);
s0313045 0:502b364c9f1d 70 //pc.printf("\r\n");
s0313045 0:502b364c9f1d 71 switch(count) {
s0313045 0:502b364c9f1d 72 case 0:
s0313045 0:502b364c9f1d 73 if (c==0x0d) count++;
s0313045 0:502b364c9f1d 74 //else count=0;
s0313045 0:502b364c9f1d 75 break;
s0313045 0:502b364c9f1d 76 case 1:
s0313045 0:502b364c9f1d 77 if(c==0x0a) {
s0313045 0:502b364c9f1d 78 i=0;
s0313045 0:502b364c9f1d 79 count++;
s0313045 0:502b364c9f1d 80 } else if(c==0x0d) {}
s0313045 0:502b364c9f1d 81 //else count=0;
s0313045 0:502b364c9f1d 82 break;
s0313045 0:502b364c9f1d 83 case 2:
s0313045 0:502b364c9f1d 84 posture.data[i]=c;
s0313045 0:502b364c9f1d 85 i++;
s0313045 0:502b364c9f1d 86 if(i>=24) {
s0313045 0:502b364c9f1d 87 i=0;
s0313045 0:502b364c9f1d 88 count++;
s0313045 0:502b364c9f1d 89 }
s0313045 0:502b364c9f1d 90 break;
s0313045 0:502b364c9f1d 91 case 3:
s0313045 0:502b364c9f1d 92 if(c==0x0a)count++;
s0313045 0:502b364c9f1d 93 //else count=0;
s0313045 0:502b364c9f1d 94 break;
s0313045 0:502b364c9f1d 95 case 4:
s0313045 0:502b364c9f1d 96 if(c==0x0d) {
s0313045 0:502b364c9f1d 97 d_angle=posture.val[0]-temp_zangle;
s0313045 0:502b364c9f1d 98 if (d_angle<-180) {
s0313045 0:502b364c9f1d 99 d_angle=d_angle+360;
s0313045 0:502b364c9f1d 100 } else if (d_angle>180) {
s0313045 0:502b364c9f1d 101 d_angle=d_angle-360;
s0313045 0:502b364c9f1d 102 }
s0313045 0:502b364c9f1d 103 zangle+=d_angle;
s0313045 0:502b364c9f1d 104 temp_zangle=posture.val[0];
s0313045 0:502b364c9f1d 105 xangle=posture.val[1];
s0313045 0:502b364c9f1d 106 yangle=posture.val[2];
s0313045 0:502b364c9f1d 107 pos_x=posture.val[3];
s0313045 0:502b364c9f1d 108 pos_y=posture.val[4];
s0313045 0:502b364c9f1d 109 angleSpeed=posture.val[5];
s0313045 0:502b364c9f1d 110 newDataArrived=true;
s0313045 0:502b364c9f1d 111
s0313045 0:502b364c9f1d 112 pc.printf("GET\r\n");
s0313045 0:502b364c9f1d 113 sprintf(buffer, "%f", pos_x);
s0313045 0:502b364c9f1d 114 pc.printf(buffer);
s0313045 0:502b364c9f1d 115 pc.printf(" ");
s0313045 0:502b364c9f1d 116 sprintf(buffer, "%f", pos_y);
s0313045 0:502b364c9f1d 117 pc.printf(buffer);
s0313045 0:502b364c9f1d 118 pc.printf(" ");
s0313045 0:502b364c9f1d 119 sprintf(buffer, "%f", zangle);
s0313045 0:502b364c9f1d 120 pc.printf(buffer);
s0313045 0:502b364c9f1d 121 pc.printf("\r\n");
s0313045 0:502b364c9f1d 122 }
s0313045 0:502b364c9f1d 123 count=0;
s0313045 0:502b364c9f1d 124 done=1;
s0313045 0:502b364c9f1d 125 LastRead=millis();
s0313045 0:502b364c9f1d 126 break;
s0313045 0:502b364c9f1d 127 default:
s0313045 0:502b364c9f1d 128 //count=0;
s0313045 0:502b364c9f1d 129 break;
s0313045 0:502b364c9f1d 130 }
s0313045 0:502b364c9f1d 131 }
s0313045 0:502b364c9f1d 132
s0313045 0:502b364c9f1d 133 bool updated()
s0313045 0:502b364c9f1d 134 {
s0313045 0:502b364c9f1d 135 if (done) {
s0313045 0:502b364c9f1d 136 done=false;
s0313045 0:502b364c9f1d 137 return true;
s0313045 0:502b364c9f1d 138 } else {
s0313045 0:502b364c9f1d 139 return false;
s0313045 0:502b364c9f1d 140 }
s0313045 0:502b364c9f1d 141
s0313045 0:502b364c9f1d 142 }
s0313045 0:502b364c9f1d 143
s0313045 0:502b364c9f1d 144 float getXangle()
s0313045 0:502b364c9f1d 145 {
s0313045 0:502b364c9f1d 146 return xangle;
s0313045 0:502b364c9f1d 147 }
s0313045 0:502b364c9f1d 148
s0313045 0:502b364c9f1d 149 float getYangle()
s0313045 0:502b364c9f1d 150 {
s0313045 0:502b364c9f1d 151 return yangle;
s0313045 0:502b364c9f1d 152 }
s0313045 0:502b364c9f1d 153
s0313045 0:502b364c9f1d 154 float getZangle()
s0313045 0:502b364c9f1d 155 {
s0313045 0:502b364c9f1d 156 return zangle;
s0313045 0:502b364c9f1d 157 }
s0313045 0:502b364c9f1d 158
s0313045 0:502b364c9f1d 159 float getXpos()
s0313045 0:502b364c9f1d 160 {
s0313045 0:502b364c9f1d 161 return pos_x;
s0313045 0:502b364c9f1d 162 }
s0313045 0:502b364c9f1d 163
s0313045 0:502b364c9f1d 164 float getYpos()
s0313045 0:502b364c9f1d 165 {
s0313045 0:502b364c9f1d 166 return pos_y;
s0313045 0:502b364c9f1d 167 }
s0313045 0:502b364c9f1d 168
s0313045 0:502b364c9f1d 169 float getAngleSpeed()
s0313045 0:502b364c9f1d 170 {
s0313045 0:502b364c9f1d 171 return angleSpeed;
s0313045 0:502b364c9f1d 172 }
s0313045 0:502b364c9f1d 173
s0313045 0:502b364c9f1d 174 bool isAlive()
s0313045 0:502b364c9f1d 175 {
s0313045 0:502b364c9f1d 176 if ((millis()-LastRead)<100) {
s0313045 0:502b364c9f1d 177 return true;
s0313045 0:502b364c9f1d 178 } else {
s0313045 0:502b364c9f1d 179 return false;
s0313045 0:502b364c9f1d 180 }
s0313045 0:502b364c9f1d 181 }
s0313045 0:502b364c9f1d 182
s0313045 0:502b364c9f1d 183 bool newDataAvailable()
s0313045 0:502b364c9f1d 184 {
s0313045 0:502b364c9f1d 185 if (newDataArrived) {
s0313045 0:502b364c9f1d 186 newDataArrived=false;
s0313045 0:502b364c9f1d 187 return true;
s0313045 0:502b364c9f1d 188 } else return false;
s0313045 0:502b364c9f1d 189 }
s0313045 0:502b364c9f1d 190
s0313045 0:502b364c9f1d 191 char* reset()
s0313045 0:502b364c9f1d 192 {
s0313045 0:502b364c9f1d 193 return "ACT0";
s0313045 0:502b364c9f1d 194 }
s0313045 0:502b364c9f1d 195
s0313045 0:502b364c9f1d 196 char* calibrate()
s0313045 0:502b364c9f1d 197 {
s0313045 0:502b364c9f1d 198 return "ACTR";
s0313045 0:502b364c9f1d 199 }
s0313045 0:502b364c9f1d 200 //////////////
s0313045 0:502b364c9f1d 201
s0313045 0:502b364c9f1d 202 void action_get()
s0313045 0:502b364c9f1d 203 {
s0313045 0:502b364c9f1d 204
s0313045 0:502b364c9f1d 205 /*
s0313045 0:502b364c9f1d 206
s0313045 0:502b364c9f1d 207 // Loop just in case more than one character is in UART's receive FIFO buffer
s0313045 0:502b364c9f1d 208 // Stop if buffer full
s0313045 0:502b364c9f1d 209 while ((device.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
s0313045 0:502b364c9f1d 210 rx_buffer[rx_in] = device.getc();
s0313045 0:502b364c9f1d 211 // Uncomment to Echo to USB serial to watch data flow
s0313045 0:502b364c9f1d 212 // monitor_device.putc(rx_buffer[rx_in]);
s0313045 0:502b364c9f1d 213 rx_in = (rx_in + 1) % buffer_size;
s0313045 0:502b364c9f1d 214 }
s0313045 0:502b364c9f1d 215 */
s0313045 0:502b364c9f1d 216
s0313045 0:502b364c9f1d 217 readEncoder(Action.getc());
s0313045 0:502b364c9f1d 218
s0313045 0:502b364c9f1d 219 }
s0313045 0:502b364c9f1d 220 /////////////////////////
s0313045 0:502b364c9f1d 221
s0313045 0:502b364c9f1d 222
s0313045 0:502b364c9f1d 223
s0313045 0:502b364c9f1d 224
s0313045 0:502b364c9f1d 225
s0313045 0:502b364c9f1d 226 int main()
s0313045 0:502b364c9f1d 227 {
s0313045 0:502b364c9f1d 228 //Action.attach(&action_get, Serial::RxIrq);
s0313045 0:502b364c9f1d 229 Action.baud(115200);
s0313045 0:502b364c9f1d 230 Action.format(8,SerialBase::None,1);
s0313045 0:502b364c9f1d 231
s0313045 0:502b364c9f1d 232
s0313045 0:502b364c9f1d 233 ActionEncoder_init();
s0313045 0:502b364c9f1d 234
s0313045 0:502b364c9f1d 235 while(1) {
s0313045 0:502b364c9f1d 236
s0313045 0:502b364c9f1d 237 while (Action.readable()==1 )
s0313045 0:502b364c9f1d 238 {
s0313045 0:502b364c9f1d 239 //pc.printf("readable\r\n");
s0313045 0:502b364c9f1d 240 char c = Action.getc();
s0313045 0:502b364c9f1d 241
s0313045 0:502b364c9f1d 242 readEncoder(c);
s0313045 0:502b364c9f1d 243 //pc.putc(c);
s0313045 0:502b364c9f1d 244 //pc.printf("\r\n");
s0313045 0:502b364c9f1d 245
s0313045 0:502b364c9f1d 246
s0313045 0:502b364c9f1d 247 /*
s0313045 0:502b364c9f1d 248 sprintf(buffer, "%f", pos_x);
s0313045 0:502b364c9f1d 249 pc.printf(buffer);
s0313045 0:502b364c9f1d 250 sprintf(buffer, "%f", pos_y);
s0313045 0:502b364c9f1d 251 pc.printf(buffer);
s0313045 0:502b364c9f1d 252 sprintf(buffer, "%f", zangle);
s0313045 0:502b364c9f1d 253 pc.printf(buffer);
s0313045 0:502b364c9f1d 254 pc.printf("\r\n");*/
s0313045 0:502b364c9f1d 255
s0313045 0:502b364c9f1d 256
s0313045 0:502b364c9f1d 257 }
s0313045 0:502b364c9f1d 258
s0313045 0:502b364c9f1d 259 }
s0313045 0:502b364c9f1d 260
s0313045 0:502b364c9f1d 261
s0313045 0:502b364c9f1d 262
s0313045 0:502b364c9f1d 263 }
s0313045 0:502b364c9f1d 264
s0313045 0:502b364c9f1d 265