Hiromasa Oku / Mbed 2 deprecated linearMirrorMotion1017

this version 10/17

Dependencies:   mbed

Fork of linearMirrorMotion by Alvaro Cassinelli

main.cpp@9:4db47b8eb750, 2013-03-06 (annotated)

Committer:
hiromasaoku
Date:
Wed Mar 06 06:59:06 2013 +0000
Revision:
9:4db47b8eb750
Parent:
8:617dbd2b6512
Child:
10:a3dd8ec4be60
working version when I just came back to Todai

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