Takashi Asano / Mbed 2 deprecated linearMirrorMotion1017

sotsuron

Fork of linearMirrorMotion1017 by Hiromasa Oku

main.cpp@6:a82917b3b1aa, 2012-12-05 (annotated)

Committer:
hiromasaoku
Date:
Wed Dec 05 08:04:25 2012 +0000
Revision:
6:a82917b3b1aa
Parent:
5:e02cd57242c7
Child:
7:64b313c58420
fighting

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