Takashi Asano
/
linearMirrorMotion1017
sotsuron
Fork of linearMirrorMotion1017 by
main.cpp
- Committer:
- hiromasaoku
- Date:
- 2013-03-01
- Revision:
- 8:617dbd2b6512
- Parent:
- 7:64b313c58420
- Child:
- 9:4db47b8eb750
File content as of revision 8:617dbd2b6512:
/* ~~~~~~present status~~~~~~ * X Y data from sereal. vx and vy are culculated like -> (new X,Y) - (previous X,Y) / timer -- i don't know how to send data quickly now I'm fighting to other problems... */ #include "mbed.h" #include "laserProjectorHardware.h" #include <vector> #define PI 3.141592 Ticker changeXY; void processSerial(); Timer timer; LocalFileSystem local("local"); //Serial pc(USBTX, USBRX); unsigned int X, Y, T; // these datas are from other tracking system... unsigned int beforeX , beforeY; int startX = CENTER_AD_MIRROR_X; int startY = CENTER_AD_MIRROR_Y; float sint=0, cost=1; float vx = 0, vy=0;//omegaX=0, omegaY=0; float theta=0, dt_betwords=200, st=400, radious=1500; unsigned int dt=500, timecounter = 0, ticktime=600; bool newPositionReady=false; unsigned int counter=0; vector<char> inputletters; bool start=false; struct point2d { int x,y; }; struct letter { int pointnum; vector<point2d> letpoints; }; //int points[200][2] = {}; vector<letter> libletter; // letter library vector<int> buf; //buffer vector vector<int> buf2; /*void makeBuffer() { float sint = sin(theta); float cost = cos(theta); startX = CENTER_AD_MIRROR_X; startY = CENTER_AD_MIRROR_Y; buf.clear(); /*conversion points to buf before writing; for(int i=0; i<inputletters.size(); i++) { buf.push_back(222); buf.push_back(222); for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) { int x = 0.8*dt_betwords/1000*(libletter[inputletters[i]-'a'].letpoints[j].x +500*(i-inputletters.size()/2.0)); int y = -dt_betwords/1000*(libletter[inputletters[i]-'a'].letpoints[j].y); buf.push_back(x); buf.push_back(y); } } } */ void CircleXY(){ X = CENTER_AD_MIRROR_X + radious * cos(2*PI*(float)timecounter/(float)ticktime); Y = CENTER_AD_MIRROR_X + radious * sin(2*PI*(float)timecounter/(float)ticktime); vx = radious * (float)PI * (-sin(2*PI*(float)timecounter/(float)ticktime)); vy = radious * (float)PI * cos(2*PI*(float)timecounter/(float)ticktime); theta = atan2(vy,vx); cost = -cos(theta); sint = -sin(theta); timecounter ++; } int main(){ FILE *fp = fopen("/local/test.txt", "r"); if(!fp) { IO.setGreenPower(1); exit(1); } int letternum; fscanf(fp, "%d", &letternum); for(int i=0; i<letternum; i++) { letter bufl; fscanf(fp, "%d", &bufl.pointnum); for(int j=0; j<bufl.pointnum; j++) { point2d bufp; fscanf(fp, "%d", &bufp.x); fscanf(fp, "%d", &bufp.y); bufl.letpoints.push_back(bufp); } libletter.push_back(bufl); } //makeBuffer(); // SETUP: -------------------------------------------------------------------------------------------- IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization // Set displaying laser powers: IO.setRedPower(0); IO.setGreenPower(0);//turn on the green (displaying) laser wait_ms(100); X = beforeX = CENTER_AD_MIRROR_X; Y = beforeY = CENTER_AD_MIRROR_Y; timer.start(); //changeXY.attach_us(&CircleXY, 200); // MAIN LOOP: -------------------------------------------------------------------------------------------- while(1) { if (pc.readable()>0) processSerial(); if(start) { timer.reset(); wait_us(5000); for(int i=0; i<inputletters.size(); i++) { for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) { if (pc.readable()>0) processSerial(); 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. int y = (libletter[inputletters[i]-'a'].letpoints[j].y); IO.writeOutXY(X+cost*x - sint*y,Y + sint*x + cost*y); timer.reset(); wait_us(dt); IO.setRGBPower(3);//on } IO.setRGBPower(1); //off wait_us(dt_betwords/sqrtf(vx*vx+vy*vy)*100000); } IO.setRGBPower(1); //off wait_us(10000); start=false; } /*for(int j=0; j<buf.size()/2; j++) { if ( (buf[j*2] == 222) && (buf[j*2+1] == 222)){ IO.setRGBPower(1); int xx=buf[j*2+2]- vx*dt/1000*j; int yy=buf[j*2+3];//- vy*dt/1000*j; IO.writeOutXY(startX+cost*xx + sint*yy,startY-sint*xx + cost*yy); wait(0.0004); IO.setRGBPower(0); } else{ timer.start(); int xx=buf[j*2]- vx*dt/1000*j; int yy=buf[j*2+1];//- vy*dt/1000*j; IO.writeOutXY(startX+cost*xx + sint*yy,startY-sint*xx + cost*yy); wait_us(dt); IO.setRGBPower(0); //on timer.stop(); //dt=timer.read(); } timecounter += dt; if(timecounter >= T) timecounter = 0; }*/ //buf2.clear(); //wait(0.02); } } // -------------------------------------------------------------------------------------------- // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly: 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... int indexStringData=0;//position of the byte in the string void processSerial() { start=true; while(pc.readable()>0) { char val =pc.getc(); // Save ASCII numeric characters (ASCII 0 - 9) on stringData: if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included) stringData[indexStringData] = val; indexStringData++; } else if ((val >= 'a') && (val <= 'z')) { // this is 45 to 57 (included) inputletters.push_back(val); } /*else if (val == '/') { makeBuffer(); }*/ else if (val == '.') { inputletters.clear(); } else if (val == 'X') { beforeX = X; stringData[indexStringData] = 0; X = atoi(stringData); indexStringData=0; vx = ((float)X-(float)beforeX) / (float)timer.read_us() *1000; } else if (val == 'Y') { beforeY = Y; stringData[indexStringData] = 0; Y = atoi(stringData); indexStringData=0; //timer.stop(); //newSpeedReady = true; //if( (Y-beforeY) > 5){ vy = ((float)Y-(float)beforeY) / (float)timer.read_us() *1000; theta=atan2(vy,vx); timer.reset(); timer.start(); sint = -sin(theta);//cos(theta); cost = -cos(theta);//-sin(theta); //} //if (cost > 0){ //sint = -sint;//cos(thet //cost = -cost;//-sin(theta); //} } else if (val == 'D') { stringData[indexStringData] = 0; dt = atoi(stringData); indexStringData=0; //makeBuffer(); } else if (val == 'B') { stringData[indexStringData] = 0; dt_betwords = atoi(stringData); indexStringData=0; //makeBuffer(); } else if (val == 'S') { stringData[indexStringData] = 0; st = atoi(stringData); indexStringData=0; //makeBuffer(); }else if (val == 'R') { stringData[indexStringData] = 0; radious = atoi(stringData); indexStringData=0; //makeBuffer(); } else if (val == 'T') { stringData[indexStringData] = 0; ticktime = atoi(stringData); indexStringData=0; //makeBuffer(); } // X value? /*else if (val=='x') { stringData[indexStringData] = 0 ; omegaX=atoi(stringData); indexStringData=0; //newPositionReady=true; } // Y value? else if (val=='y') { stringData[indexStringData] = 0 ; omegaY=atoi(stringData); indexStringData=0; makeBuffer(); newPositionReady=true; } else if (val=='g') { stringData[indexStringData] = 0 ; int power=atoi(stringData); indexStringData=0; IO.setGreenPower(power); } else if (val=='r') { stringData[indexStringData] = 0 ; int power=atoi(stringData); indexStringData=0; IO.setRedPower(power); } else if (val=='c') { stringData[indexStringData] = 0 ; int power=atoi(stringData); indexStringData=0; IO.setRGBPower(power); } */ } }