Hiromasa Oku / Mbed 2 deprecated linearMirrorMotion1017

this version 10/17

Dependencies:   mbed

Fork of linearMirrorMotion by Alvaro Cassinelli

main.cpp@5:e02cd57242c7, 2012-11-29 (annotated)

Committer:
hiromasaoku
Date:
Thu Nov 29 08:05:10 2012 +0000
Revision:
5:e02cd57242c7
Parent:
4:e00e709d7173
Child:
6:a82917b3b1aa
not completed. fighting stage

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