Takashi Asano / Mbed 2 deprecated linearMirrorMotion1017

sotsuron

Fork of linearMirrorMotion1017 by Hiromasa Oku

main.cpp@8:617dbd2b6512, 2013-03-01 (annotated)

Committer:
hiromasaoku
Date:
Fri Mar 01 08:08:28 2013 +0000
Revision:
8:617dbd2b6512
Parent:
7:64b313c58420
Child:
9:4db47b8eb750
remembering....

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiromasaoku 5:e02cd57242c7 1 /*
hiromasaoku 5:e02cd57242c7 2 ~~~~~~present status~~~~~~
hiromasaoku 5:e02cd57242c7 3 * X Y data from sereal. vx and vy are culculated like -> (new X,Y) - (previous X,Y) / timer
hiromasaoku 5:e02cd57242c7 4 -- i don't know how to send data quickly
hiromasaoku 5:e02cd57242c7 5
hiromasaoku 5:e02cd57242c7 6 now I'm fighting to other problems...
hiromasaoku 5:e02cd57242c7 7 */
hiromasaoku 5:e02cd57242c7 8
hiromasaoku 5:e02cd57242c7 9
mbedalvaro 0:4e12dea53fbe 10 #include "mbed.h"
mbedalvaro 1:daf6b4939120 11 #include "laserProjectorHardware.h"
hiromasaoku 4:e00e709d7173 12 #include <vector>
hiromasaoku 7:64b313c58420 13 #define PI 3.141592
hiromasaoku 7:64b313c58420 14
hiromasaoku 7:64b313c58420 15 Ticker changeXY;
mbedalvaro 0:4e12dea53fbe 16
mbedalvaro 0:4e12dea53fbe 17 void processSerial();
hiromasaoku 4:e00e709d7173 18 Timer timer;
hiromasaoku 4:e00e709d7173 19 LocalFileSystem local("local");
hiromasaoku 5:e02cd57242c7 20 //Serial pc(USBTX, USBRX);
mbedalvaro 0:4e12dea53fbe 21
hiromasaoku 5:e02cd57242c7 22 unsigned int X, Y, T; // these datas are from other tracking system...
hiromasaoku 5:e02cd57242c7 23 unsigned int beforeX , beforeY;
hiromasaoku 4:e00e709d7173 24 int startX = CENTER_AD_MIRROR_X;
hiromasaoku 4:e00e709d7173 25 int startY = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 26
hiromasaoku 6:a82917b3b1aa 27 float sint=0, cost=1;
hiromasaoku 6:a82917b3b1aa 28
hiromasaoku 7:64b313c58420 29 float vx = 0, vy=0;//omegaX=0, omegaY=0;
hiromasaoku 7:64b313c58420 30 float theta=0, dt_betwords=200, st=400, radious=1500;
hiromasaoku 7:64b313c58420 31 unsigned int dt=500, timecounter = 0, ticktime=600;
hiromasaoku 4:e00e709d7173 32
mbedalvaro 0:4e12dea53fbe 33 bool newPositionReady=false;
mbedalvaro 0:4e12dea53fbe 34 unsigned int counter=0;
hiromasaoku 5:e02cd57242c7 35 vector<char> inputletters;
mbedalvaro 0:4e12dea53fbe 36
hiromasaoku 6:a82917b3b1aa 37 bool start=false;
hiromasaoku 6:a82917b3b1aa 38
hiromasaoku 5:e02cd57242c7 39 struct point2d {
hiromasaoku 4:e00e709d7173 40 int x,y;
hiromasaoku 4:e00e709d7173 41 };
hiromasaoku 5:e02cd57242c7 42 struct letter {
hiromasaoku 4:e00e709d7173 43 int pointnum;
hiromasaoku 5:e02cd57242c7 44 vector<point2d> letpoints;
hiromasaoku 4:e00e709d7173 45 };
hiromasaoku 4:e00e709d7173 46
hiromasaoku 4:e00e709d7173 47 //int points[200][2] = {};
hiromasaoku 5:e02cd57242c7 48 vector<letter> libletter; // letter library
hiromasaoku 5:e02cd57242c7 49
hiromasaoku 4:e00e709d7173 50 vector<int> buf; //buffer vector
hiromasaoku 5:e02cd57242c7 51 vector<int> buf2;
hiromasaoku 4:e00e709d7173 52
hiromasaoku 5:e02cd57242c7 53 /*void makeBuffer()
hiromasaoku 4:e00e709d7173 54 {
hiromasaoku 4:e00e709d7173 55 float sint = sin(theta);
hiromasaoku 4:e00e709d7173 56 float cost = cos(theta);
hiromasaoku 4:e00e709d7173 57
hiromasaoku 4:e00e709d7173 58 startX = CENTER_AD_MIRROR_X;
hiromasaoku 4:e00e709d7173 59 startY = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 60
hiromasaoku 4:e00e709d7173 61 buf.clear();
hiromasaoku 5:e02cd57242c7 62 /*conversion points to buf before writing;
hiromasaoku 5:e02cd57242c7 63 for(int i=0; i<inputletters.size(); i++) {
hiromasaoku 5:e02cd57242c7 64
hiromasaoku 5:e02cd57242c7 65 buf.push_back(222);
hiromasaoku 5:e02cd57242c7 66 buf.push_back(222);
hiromasaoku 5:e02cd57242c7 67
hiromasaoku 5:e02cd57242c7 68 for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
hiromasaoku 7:64b313c58420 69 int x = 0.8*dt_betwords/1000*(libletter[inputletters[i]-'a'].letpoints[j].x +500*(i-inputletters.size()/2.0));
hiromasaoku 7:64b313c58420 70 int y = -dt_betwords/1000*(libletter[inputletters[i]-'a'].letpoints[j].y);
hiromasaoku 5:e02cd57242c7 71 buf.push_back(x);
hiromasaoku 5:e02cd57242c7 72 buf.push_back(y);
hiromasaoku 5:e02cd57242c7 73 }
hiromasaoku 4:e00e709d7173 74 }
hiromasaoku 5:e02cd57242c7 75
hiromasaoku 4:e00e709d7173 76 }
hiromasaoku 5:e02cd57242c7 77 */
hiromasaoku 4:e00e709d7173 78
hiromasaoku 7:64b313c58420 79 void CircleXY(){
hiromasaoku 7:64b313c58420 80 X = CENTER_AD_MIRROR_X + radious * cos(2*PI*(float)timecounter/(float)ticktime);
hiromasaoku 7:64b313c58420 81 Y = CENTER_AD_MIRROR_X + radious * sin(2*PI*(float)timecounter/(float)ticktime);
hiromasaoku 7:64b313c58420 82 vx = radious * (float)PI * (-sin(2*PI*(float)timecounter/(float)ticktime));
hiromasaoku 7:64b313c58420 83 vy = radious * (float)PI * cos(2*PI*(float)timecounter/(float)ticktime);
hiromasaoku 7:64b313c58420 84 theta = atan2(vy,vx);
hiromasaoku 7:64b313c58420 85 cost = -cos(theta);
hiromasaoku 7:64b313c58420 86 sint = -sin(theta);
hiromasaoku 7:64b313c58420 87 timecounter ++;
hiromasaoku 7:64b313c58420 88 }
hiromasaoku 7:64b313c58420 89
hiromasaoku 7:64b313c58420 90 int main(){
hiromasaoku 7:64b313c58420 91
hiromasaoku 4:e00e709d7173 92 FILE *fp = fopen("/local/test.txt", "r");
hiromasaoku 5:e02cd57242c7 93 if(!fp) {
hiromasaoku 5:e02cd57242c7 94 IO.setGreenPower(1);
hiromasaoku 5:e02cd57242c7 95 exit(1);
hiromasaoku 5:e02cd57242c7 96 }
hiromasaoku 4:e00e709d7173 97 int letternum;
hiromasaoku 4:e00e709d7173 98 fscanf(fp, "%d", &letternum);
hiromasaoku 5:e02cd57242c7 99 for(int i=0; i<letternum; i++) {
hiromasaoku 5:e02cd57242c7 100 letter bufl;
hiromasaoku 4:e00e709d7173 101 fscanf(fp, "%d", &bufl.pointnum);
hiromasaoku 5:e02cd57242c7 102 for(int j=0; j<bufl.pointnum; j++) {
hiromasaoku 5:e02cd57242c7 103 point2d bufp;
hiromasaoku 4:e00e709d7173 104 fscanf(fp, "%d", &bufp.x);
hiromasaoku 4:e00e709d7173 105 fscanf(fp, "%d", &bufp.y);
hiromasaoku 4:e00e709d7173 106 bufl.letpoints.push_back(bufp);
hiromasaoku 4:e00e709d7173 107 }
hiromasaoku 5:e02cd57242c7 108 libletter.push_back(bufl);
hiromasaoku 4:e00e709d7173 109 }
hiromasaoku 5:e02cd57242c7 110
hiromasaoku 5:e02cd57242c7 111
hiromasaoku 5:e02cd57242c7 112 //makeBuffer();
mbedalvaro 0:4e12dea53fbe 113
mbedalvaro 0:4e12dea53fbe 114 // SETUP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 115 IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
hiromasaoku 4:e00e709d7173 116
hiromasaoku 4:e00e709d7173 117 // Set displaying laser powers:
mbedalvaro 0:4e12dea53fbe 118 IO.setRedPower(0);
mbedalvaro 0:4e12dea53fbe 119 IO.setGreenPower(0);//turn on the green (displaying) laser
mbedalvaro 0:4e12dea53fbe 120
mbedalvaro 0:4e12dea53fbe 121 wait_ms(100);
mbedalvaro 0:4e12dea53fbe 122
hiromasaoku 5:e02cd57242c7 123 X = beforeX = CENTER_AD_MIRROR_X;
hiromasaoku 5:e02cd57242c7 124 Y = beforeY = CENTER_AD_MIRROR_Y;
hiromasaoku 5:e02cd57242c7 125 timer.start();
hiromasaoku 8:617dbd2b6512 126 //changeXY.attach_us(&CircleXY, 200);
mbedalvaro 0:4e12dea53fbe 127 // MAIN LOOP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 128 while(1) {
hiromasaoku 6:a82917b3b1aa 129 if (pc.readable()>0) processSerial();
hiromasaoku 6:a82917b3b1aa 130
hiromasaoku 6:a82917b3b1aa 131 if(start) {
hiromasaoku 7:64b313c58420 132 timer.reset();
hiromasaoku 6:a82917b3b1aa 133 wait_us(5000);
hiromasaoku 5:e02cd57242c7 134 for(int i=0; i<inputletters.size(); i++) {
hiromasaoku 6:a82917b3b1aa 135
hiromasaoku 6:a82917b3b1aa 136
hiromasaoku 5:e02cd57242c7 137 for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
hiromasaoku 6:a82917b3b1aa 138 if (pc.readable()>0) processSerial();
hiromasaoku 6:a82917b3b1aa 139
hiromasaoku 7:64b313c58420 140 int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - sqrtf(vx*vx+vy*vy)*timer.read_us()/st*j/100); //this ZURE should be modifyed not by dt but Timer.
hiromasaoku 6:a82917b3b1aa 141 int y = (libletter[inputletters[i]-'a'].letpoints[j].y);
hiromasaoku 6:a82917b3b1aa 142 IO.writeOutXY(X+cost*x - sint*y,Y + sint*x + cost*y);
hiromasaoku 7:64b313c58420 143 timer.reset();
hiromasaoku 5:e02cd57242c7 144 wait_us(dt);
hiromasaoku 6:a82917b3b1aa 145 IO.setRGBPower(3);//on
hiromasaoku 5:e02cd57242c7 146 }
hiromasaoku 6:a82917b3b1aa 147 IO.setRGBPower(1); //off
hiromasaoku 7:64b313c58420 148 wait_us(dt_betwords/sqrtf(vx*vx+vy*vy)*100000);
hiromasaoku 6:a82917b3b1aa 149
hiromasaoku 5:e02cd57242c7 150 }
hiromasaoku 6:a82917b3b1aa 151
hiromasaoku 6:a82917b3b1aa 152 IO.setRGBPower(1); //off
hiromasaoku 6:a82917b3b1aa 153 wait_us(10000);
hiromasaoku 6:a82917b3b1aa 154 start=false;
hiromasaoku 6:a82917b3b1aa 155
hiromasaoku 6:a82917b3b1aa 156 }
hiromasaoku 6:a82917b3b1aa 157
hiromasaoku 5:e02cd57242c7 158 /*for(int j=0; j<buf.size()/2; j++) {
hiromasaoku 5:e02cd57242c7 159 if ( (buf[j*2] == 222) && (buf[j*2+1] == 222)){
hiromasaoku 5:e02cd57242c7 160 IO.setRGBPower(1);
hiromasaoku 5:e02cd57242c7 161 int xx=buf[j*2+2]- vx*dt/1000*j;
hiromasaoku 5:e02cd57242c7 162 int yy=buf[j*2+3];//- vy*dt/1000*j;
hiromasaoku 5:e02cd57242c7 163 IO.writeOutXY(startX+cost*xx + sint*yy,startY-sint*xx + cost*yy);
hiromasaoku 5:e02cd57242c7 164 wait(0.0004);
hiromasaoku 5:e02cd57242c7 165 IO.setRGBPower(0);
hiromasaoku 5:e02cd57242c7 166 }
hiromasaoku 5:e02cd57242c7 167 else{
hiromasaoku 4:e00e709d7173 168 timer.start();
hiromasaoku 5:e02cd57242c7 169 int xx=buf[j*2]- vx*dt/1000*j;
hiromasaoku 5:e02cd57242c7 170 int yy=buf[j*2+1];//- vy*dt/1000*j;
hiromasaoku 5:e02cd57242c7 171 IO.writeOutXY(startX+cost*xx + sint*yy,startY-sint*xx + cost*yy);
hiromasaoku 4:e00e709d7173 172 wait_us(dt);
hiromasaoku 4:e00e709d7173 173 IO.setRGBPower(0); //on
hiromasaoku 4:e00e709d7173 174 timer.stop();
hiromasaoku 4:e00e709d7173 175 //dt=timer.read();
hiromasaoku 5:e02cd57242c7 176 }
hiromasaoku 5:e02cd57242c7 177
hiromasaoku 5:e02cd57242c7 178 timecounter += dt;
hiromasaoku 5:e02cd57242c7 179 if(timecounter >= T) timecounter = 0;
hiromasaoku 5:e02cd57242c7 180 }*/
hiromasaoku 6:a82917b3b1aa 181 //buf2.clear();
hiromasaoku 6:a82917b3b1aa 182 //wait(0.02);
hiromasaoku 4:e00e709d7173 183
mbedalvaro 0:4e12dea53fbe 184 }
mbedalvaro 0:4e12dea53fbe 185 }
mbedalvaro 0:4e12dea53fbe 186
hiromasaoku 4:e00e709d7173 187
mbedalvaro 0:4e12dea53fbe 188 // --------------------------------------------------------------------------------------------
hiromasaoku 4:e00e709d7173 189 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
mbedalvaro 0:4e12dea53fbe 190 char stringData[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int...
mbedalvaro 0:4e12dea53fbe 191 int indexStringData=0;//position of the byte in the string
mbedalvaro 0:4e12dea53fbe 192
hiromasaoku 4:e00e709d7173 193 void processSerial()
hiromasaoku 4:e00e709d7173 194 {
hiromasaoku 4:e00e709d7173 195
hiromasaoku 6:a82917b3b1aa 196 start=true;
hiromasaoku 4:e00e709d7173 197 while(pc.readable()>0) {
hiromasaoku 4:e00e709d7173 198
hiromasaoku 4:e00e709d7173 199 char val =pc.getc();
hiromasaoku 4:e00e709d7173 200
hiromasaoku 4:e00e709d7173 201 // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
hiromasaoku 4:e00e709d7173 202 if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
hiromasaoku 4:e00e709d7173 203 stringData[indexStringData] = val;
hiromasaoku 4:e00e709d7173 204 indexStringData++;
hiromasaoku 5:e02cd57242c7 205 } else if ((val >= 'a') && (val <= 'z')) { // this is 45 to 57 (included)
hiromasaoku 5:e02cd57242c7 206 inputletters.push_back(val);
hiromasaoku 5:e02cd57242c7 207 }
hiromasaoku 5:e02cd57242c7 208
hiromasaoku 5:e02cd57242c7 209 /*else if (val == '/') {
hiromasaoku 5:e02cd57242c7 210 makeBuffer();
hiromasaoku 5:e02cd57242c7 211 }*/ else if (val == '.') {
hiromasaoku 5:e02cd57242c7 212 inputletters.clear();
hiromasaoku 4:e00e709d7173 213 }
mbedalvaro 0:4e12dea53fbe 214
hiromasaoku 4:e00e709d7173 215 else if (val == 'X') {
hiromasaoku 5:e02cd57242c7 216 beforeX = X;
hiromasaoku 4:e00e709d7173 217 stringData[indexStringData] = 0;
hiromasaoku 5:e02cd57242c7 218 X = atoi(stringData);
hiromasaoku 4:e00e709d7173 219 indexStringData=0;
hiromasaoku 7:64b313c58420 220 vx = ((float)X-(float)beforeX) / (float)timer.read_us() *1000;
hiromasaoku 4:e00e709d7173 221 }
hiromasaoku 4:e00e709d7173 222
hiromasaoku 4:e00e709d7173 223 else if (val == 'Y') {
hiromasaoku 5:e02cd57242c7 224 beforeY = Y;
hiromasaoku 4:e00e709d7173 225 stringData[indexStringData] = 0;
hiromasaoku 5:e02cd57242c7 226 Y = atoi(stringData);
hiromasaoku 4:e00e709d7173 227 indexStringData=0;
hiromasaoku 6:a82917b3b1aa 228 //timer.stop();
hiromasaoku 4:e00e709d7173 229 //newSpeedReady = true;
hiromasaoku 6:a82917b3b1aa 230 //if( (Y-beforeY) > 5){
hiromasaoku 7:64b313c58420 231 vy = ((float)Y-(float)beforeY) / (float)timer.read_us() *1000;
hiromasaoku 6:a82917b3b1aa 232 theta=atan2(vy,vx);
hiromasaoku 6:a82917b3b1aa 233 timer.reset();
hiromasaoku 5:e02cd57242c7 234 timer.start();
hiromasaoku 6:a82917b3b1aa 235 sint = -sin(theta);//cos(theta);
hiromasaoku 6:a82917b3b1aa 236 cost = -cos(theta);//-sin(theta);
hiromasaoku 6:a82917b3b1aa 237 //}
hiromasaoku 6:a82917b3b1aa 238 //if (cost > 0){
hiromasaoku 6:a82917b3b1aa 239 //sint = -sint;//cos(thet
hiromasaoku 6:a82917b3b1aa 240 //cost = -cost;//-sin(theta);
hiromasaoku 6:a82917b3b1aa 241 //}
hiromasaoku 4:e00e709d7173 242 }
mbedalvaro 2:0548c7bf9fba 243
hiromasaoku 7:64b313c58420 244 else if (val == 'D') {
hiromasaoku 4:e00e709d7173 245 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 246 dt = atoi(stringData);
hiromasaoku 4:e00e709d7173 247 indexStringData=0;
hiromasaoku 5:e02cd57242c7 248 //makeBuffer();
hiromasaoku 7:64b313c58420 249 } else if (val == 'B') {
hiromasaoku 7:64b313c58420 250 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 251 dt_betwords = atoi(stringData);
hiromasaoku 7:64b313c58420 252 indexStringData=0;
hiromasaoku 7:64b313c58420 253 //makeBuffer();
hiromasaoku 5:e02cd57242c7 254 } else if (val == 'S') {
hiromasaoku 4:e00e709d7173 255 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 256 st = atoi(stringData);
hiromasaoku 7:64b313c58420 257 indexStringData=0;
hiromasaoku 7:64b313c58420 258 //makeBuffer();
hiromasaoku 7:64b313c58420 259 }else if (val == 'R') {
hiromasaoku 7:64b313c58420 260 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 261 radious = atoi(stringData);
hiromasaoku 7:64b313c58420 262 indexStringData=0;
hiromasaoku 7:64b313c58420 263 //makeBuffer();
hiromasaoku 7:64b313c58420 264 } else if (val == 'T') {
hiromasaoku 7:64b313c58420 265 stringData[indexStringData] = 0;
hiromasaoku 7:64b313c58420 266 ticktime = atoi(stringData);
hiromasaoku 4:e00e709d7173 267 indexStringData=0;
hiromasaoku 5:e02cd57242c7 268 //makeBuffer();
hiromasaoku 5:e02cd57242c7 269 }
hiromasaoku 4:e00e709d7173 270 // X value?
hiromasaoku 5:e02cd57242c7 271 /*else if (val=='x') {
hiromasaoku 4:e00e709d7173 272 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 273 omegaX=atoi(stringData);
hiromasaoku 4:e00e709d7173 274 indexStringData=0;
hiromasaoku 4:e00e709d7173 275 //newPositionReady=true;
hiromasaoku 4:e00e709d7173 276 }
hiromasaoku 4:e00e709d7173 277 // Y value?
hiromasaoku 4:e00e709d7173 278 else if (val=='y') {
hiromasaoku 4:e00e709d7173 279 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 280 omegaY=atoi(stringData);
hiromasaoku 4:e00e709d7173 281 indexStringData=0;
hiromasaoku 5:e02cd57242c7 282 makeBuffer();
hiromasaoku 4:e00e709d7173 283 newPositionReady=true;
hiromasaoku 4:e00e709d7173 284 }
hiromasaoku 4:e00e709d7173 285
hiromasaoku 4:e00e709d7173 286 else if (val=='g') {
hiromasaoku 4:e00e709d7173 287 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 288 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 289 indexStringData=0;
hiromasaoku 4:e00e709d7173 290 IO.setGreenPower(power);
hiromasaoku 4:e00e709d7173 291 } else if (val=='r') {
hiromasaoku 4:e00e709d7173 292 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 293 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 294 indexStringData=0;
hiromasaoku 4:e00e709d7173 295 IO.setRedPower(power);
hiromasaoku 4:e00e709d7173 296 } else if (val=='c') {
hiromasaoku 4:e00e709d7173 297 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 298 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 299 indexStringData=0;
hiromasaoku 4:e00e709d7173 300 IO.setRGBPower(power);
hiromasaoku 4:e00e709d7173 301 }
hiromasaoku 5:e02cd57242c7 302 */
hiromasaoku 4:e00e709d7173 303
hiromasaoku 6:a82917b3b1aa 304 }
mbedalvaro 0:4e12dea53fbe 305 }