Hiromasa Oku / Mbed 2 deprecated linearMirrorMotion1017

this version 10/17

Dependencies:   mbed

Fork of linearMirrorMotion by Alvaro Cassinelli

main.cpp@4:e00e709d7173, 2012-10-24 (annotated)

Committer:
hiromasaoku
Date:
Wed Oct 24 07:17:26 2012 +0000
Revision:
4:e00e709d7173
Parent:
2:0548c7bf9fba
Child:
5:e02cd57242c7
this is Asano code, not finished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 0:4e12dea53fbe 1 #include "mbed.h"
mbedalvaro 1:daf6b4939120 2 #include "laserProjectorHardware.h"
hiromasaoku 4:e00e709d7173 3 #include <vector>
hiromasaoku 4:e00e709d7173 4 /*yarukoto
hiromasaoku 4:e00e709d7173 5 ~vx,vy,theta,scale : parameta de kaeru.
hiromasaoku 4:e00e709d7173 6 ~letters.txt tsukuru.
hiromasaoku 4:e00e709d7173 7 ~realistic na kanji ni suru omega,dist toka.
hiromasaoku 4:e00e709d7173 8 */
mbedalvaro 0:4e12dea53fbe 9
mbedalvaro 0:4e12dea53fbe 10 void processSerial();
hiromasaoku 4:e00e709d7173 11 Timer timer;
hiromasaoku 4:e00e709d7173 12 LocalFileSystem local("local");
mbedalvaro 0:4e12dea53fbe 13
hiromasaoku 4:e00e709d7173 14 unsigned int X, Y;
hiromasaoku 4:e00e709d7173 15 int startX = CENTER_AD_MIRROR_X;
hiromasaoku 4:e00e709d7173 16 int startY = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 17
hiromasaoku 4:e00e709d7173 18 float vx = 0, vy=0, omegaX=0, omegaY=0, dist=1000; // these data may be from saccade mirror...
hiromasaoku 4:e00e709d7173 19 float theta=0,scale=2000;
hiromasaoku 4:e00e709d7173 20 unsigned int dt=30;
hiromasaoku 4:e00e709d7173 21
mbedalvaro 0:4e12dea53fbe 22 bool newPositionReady=false;
mbedalvaro 0:4e12dea53fbe 23 unsigned int counter=0;
mbedalvaro 0:4e12dea53fbe 24
hiromasaoku 4:e00e709d7173 25 struct point{
hiromasaoku 4:e00e709d7173 26 int x,y;
hiromasaoku 4:e00e709d7173 27 };
hiromasaoku 4:e00e709d7173 28
hiromasaoku 4:e00e709d7173 29 struct letter{
hiromasaoku 4:e00e709d7173 30 int pointnum;
hiromasaoku 4:e00e709d7173 31 vector<struct point> letpoints;
hiromasaoku 4:e00e709d7173 32 };
hiromasaoku 4:e00e709d7173 33
hiromasaoku 4:e00e709d7173 34 //int points[200][2] = {};
hiromasaoku 4:e00e709d7173 35 vector<struct letter> points; // drawing points
hiromasaoku 4:e00e709d7173 36
hiromasaoku 4:e00e709d7173 37 vector<int> buf; //buffer vector
hiromasaoku 4:e00e709d7173 38
hiromasaoku 4:e00e709d7173 39 void recomputeBuffer()
hiromasaoku 4:e00e709d7173 40 {
hiromasaoku 4:e00e709d7173 41 float sint = sin(theta);
hiromasaoku 4:e00e709d7173 42 float cost = cos(theta);
hiromasaoku 4:e00e709d7173 43
hiromasaoku 4:e00e709d7173 44 startX = CENTER_AD_MIRROR_X;
hiromasaoku 4:e00e709d7173 45 startY = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 46
hiromasaoku 4:e00e709d7173 47 buf.clear();
hiromasaoku 4:e00e709d7173 48 //conversion points to buf before writing;
hiromasaoku 4:e00e709d7173 49 for(int j=0; j<3; j++) {
hiromasaoku 4:e00e709d7173 50 int x = scale/(dist+tan(omegaX/360*2*3.1415)*points[0].letpoints[j].x)*points[0].letpoints[j].x - vx*dt/1000*j;
hiromasaoku 4:e00e709d7173 51 int y = scale/(dist+tan(omegaY/360*2*3.1415)*points[0].letpoints[j].y)*points[0].letpoints[j].y - vy*dt/1000*j;
hiromasaoku 4:e00e709d7173 52 buf.push_back(startX+cost*x + sint*y);
hiromasaoku 4:e00e709d7173 53 buf.push_back(startY-sint*x + cost*y);
hiromasaoku 4:e00e709d7173 54 }
hiromasaoku 4:e00e709d7173 55 }
hiromasaoku 4:e00e709d7173 56
hiromasaoku 4:e00e709d7173 57
hiromasaoku 4:e00e709d7173 58 int main()
hiromasaoku 4:e00e709d7173 59 {
hiromasaoku 4:e00e709d7173 60 FILE *fp = fopen("/local/test.txt", "r");
hiromasaoku 4:e00e709d7173 61 if(!fp) {IO.setGreenPower(1); exit(1);}
hiromasaoku 4:e00e709d7173 62 int letternum;
hiromasaoku 4:e00e709d7173 63 fscanf(fp, "%d", &letternum);
hiromasaoku 4:e00e709d7173 64 for(int i=0;i<letternum;i++){
hiromasaoku 4:e00e709d7173 65 struct letter bufl;
hiromasaoku 4:e00e709d7173 66 fscanf(fp, "%d", &bufl.pointnum);
hiromasaoku 4:e00e709d7173 67 for(int j=0;j<bufl.pointnum;j++){
hiromasaoku 4:e00e709d7173 68 struct point bufp;
hiromasaoku 4:e00e709d7173 69 fscanf(fp, "%d", &bufp.x);
hiromasaoku 4:e00e709d7173 70 fscanf(fp, "%d", &bufp.y);
hiromasaoku 4:e00e709d7173 71 bufl.letpoints.push_back(bufp);
hiromasaoku 4:e00e709d7173 72 }
hiromasaoku 4:e00e709d7173 73 }
hiromasaoku 4:e00e709d7173 74
hiromasaoku 4:e00e709d7173 75
hiromasaoku 4:e00e709d7173 76
hiromasaoku 4:e00e709d7173 77 /*for(int i=0 ; i<200 ; i++) {
hiromasaoku 4:e00e709d7173 78 points[i][0] = 200*cos(1.0*i/200*2*3.1415);
hiromasaoku 4:e00e709d7173 79 points[i][1] = 200*sin(1.0*i/200*2*3.1415);
hiromasaoku 4:e00e709d7173 80 }*/
hiromasaoku 4:e00e709d7173 81 recomputeBuffer();
mbedalvaro 0:4e12dea53fbe 82
mbedalvaro 0:4e12dea53fbe 83 // SETUP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 84 IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
hiromasaoku 4:e00e709d7173 85
hiromasaoku 4:e00e709d7173 86 // Set displaying laser powers:
mbedalvaro 0:4e12dea53fbe 87 IO.setRedPower(0);
mbedalvaro 0:4e12dea53fbe 88 IO.setGreenPower(0);//turn on the green (displaying) laser
mbedalvaro 0:4e12dea53fbe 89
mbedalvaro 0:4e12dea53fbe 90 wait_ms(100);
mbedalvaro 0:4e12dea53fbe 91
hiromasaoku 4:e00e709d7173 92 X = CENTER_AD_MIRROR_X;
mbedalvaro 0:4e12dea53fbe 93 Y = CENTER_AD_MIRROR_Y;
hiromasaoku 4:e00e709d7173 94 IO.writeOutXY(X,Y);
hiromasaoku 4:e00e709d7173 95
mbedalvaro 0:4e12dea53fbe 96 // MAIN LOOP: --------------------------------------------------------------------------------------------
mbedalvaro 0:4e12dea53fbe 97 while(1) {
hiromasaoku 4:e00e709d7173 98 if (pc.readable()>0) processSerial();
hiromasaoku 4:e00e709d7173 99
hiromasaoku 4:e00e709d7173 100 for(int j=0; j<buf.size(); j++) {
hiromasaoku 4:e00e709d7173 101 timer.start();
hiromasaoku 4:e00e709d7173 102 IO.writeOutXY(buf[j*2],buf[j*2+1]);
hiromasaoku 4:e00e709d7173 103 wait_us(dt);
hiromasaoku 4:e00e709d7173 104 IO.setRGBPower(0); //on
hiromasaoku 4:e00e709d7173 105 timer.stop();
hiromasaoku 4:e00e709d7173 106 //dt=timer.read();
mbedalvaro 0:4e12dea53fbe 107 }
hiromasaoku 4:e00e709d7173 108
hiromasaoku 4:e00e709d7173 109 IO.setRGBPower(1); //off
hiromasaoku 4:e00e709d7173 110 wait(0.02);
hiromasaoku 4:e00e709d7173 111
mbedalvaro 0:4e12dea53fbe 112 }
mbedalvaro 0:4e12dea53fbe 113 }
mbedalvaro 0:4e12dea53fbe 114
hiromasaoku 4:e00e709d7173 115
mbedalvaro 0:4e12dea53fbe 116 // --------------------------------------------------------------------------------------------
hiromasaoku 4:e00e709d7173 117 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
mbedalvaro 0:4e12dea53fbe 118 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 119 int indexStringData=0;//position of the byte in the string
mbedalvaro 0:4e12dea53fbe 120
hiromasaoku 4:e00e709d7173 121 void processSerial()
hiromasaoku 4:e00e709d7173 122 {
hiromasaoku 4:e00e709d7173 123
hiromasaoku 4:e00e709d7173 124 while(pc.readable()>0) {
hiromasaoku 4:e00e709d7173 125
hiromasaoku 4:e00e709d7173 126 char val =pc.getc();
hiromasaoku 4:e00e709d7173 127
hiromasaoku 4:e00e709d7173 128 // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
hiromasaoku 4:e00e709d7173 129 if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
hiromasaoku 4:e00e709d7173 130 stringData[indexStringData] = val;
hiromasaoku 4:e00e709d7173 131 indexStringData++;
hiromasaoku 4:e00e709d7173 132 }
mbedalvaro 0:4e12dea53fbe 133
hiromasaoku 4:e00e709d7173 134 else if (val == 'X') {
hiromasaoku 4:e00e709d7173 135 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 136 vx = atoi(stringData);
hiromasaoku 4:e00e709d7173 137 indexStringData=0;
hiromasaoku 4:e00e709d7173 138 //newSpeedReady = true;
hiromasaoku 4:e00e709d7173 139 }
hiromasaoku 4:e00e709d7173 140
hiromasaoku 4:e00e709d7173 141 else if (val == 'Y') {
hiromasaoku 4:e00e709d7173 142 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 143 vy = atoi(stringData);
hiromasaoku 4:e00e709d7173 144 indexStringData=0;
hiromasaoku 4:e00e709d7173 145 //newSpeedReady = true;
hiromasaoku 4:e00e709d7173 146 theta=atan2(vy,vx);
hiromasaoku 4:e00e709d7173 147 recomputeBuffer();
hiromasaoku 4:e00e709d7173 148 }
mbedalvaro 2:0548c7bf9fba 149
hiromasaoku 4:e00e709d7173 150 else if (val == 'T') {
hiromasaoku 4:e00e709d7173 151 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 152 dt = atoi(stringData);
hiromasaoku 4:e00e709d7173 153 indexStringData=0;
hiromasaoku 4:e00e709d7173 154 recomputeBuffer();
hiromasaoku 4:e00e709d7173 155 }
hiromasaoku 4:e00e709d7173 156 else if (val == 'S') {
hiromasaoku 4:e00e709d7173 157 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 158 scale = atoi(stringData);
hiromasaoku 4:e00e709d7173 159 indexStringData=0;
hiromasaoku 4:e00e709d7173 160 recomputeBuffer();
hiromasaoku 4:e00e709d7173 161 }
hiromasaoku 4:e00e709d7173 162 else if (val == 'D') {
hiromasaoku 4:e00e709d7173 163 stringData[indexStringData] = 0;
hiromasaoku 4:e00e709d7173 164 dist = atoi(stringData);
hiromasaoku 4:e00e709d7173 165 indexStringData=0;
hiromasaoku 4:e00e709d7173 166 recomputeBuffer();
hiromasaoku 4:e00e709d7173 167 }
hiromasaoku 4:e00e709d7173 168 // X value?
hiromasaoku 4:e00e709d7173 169 else if (val=='x') {
hiromasaoku 4:e00e709d7173 170 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 171 omegaX=atoi(stringData);
hiromasaoku 4:e00e709d7173 172 indexStringData=0;
hiromasaoku 4:e00e709d7173 173 //newPositionReady=true;
hiromasaoku 4:e00e709d7173 174 }
hiromasaoku 4:e00e709d7173 175 // Y value?
hiromasaoku 4:e00e709d7173 176 else if (val=='y') {
hiromasaoku 4:e00e709d7173 177 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 178 omegaY=atoi(stringData);
hiromasaoku 4:e00e709d7173 179 indexStringData=0;
hiromasaoku 4:e00e709d7173 180 recomputeBuffer();
hiromasaoku 4:e00e709d7173 181 newPositionReady=true;
hiromasaoku 4:e00e709d7173 182 }
hiromasaoku 4:e00e709d7173 183
hiromasaoku 4:e00e709d7173 184 else if (val=='g') {
hiromasaoku 4:e00e709d7173 185 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 186 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 187 indexStringData=0;
hiromasaoku 4:e00e709d7173 188 IO.setGreenPower(power);
hiromasaoku 4:e00e709d7173 189 } else if (val=='r') {
hiromasaoku 4:e00e709d7173 190 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 191 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 192 indexStringData=0;
hiromasaoku 4:e00e709d7173 193 IO.setRedPower(power);
hiromasaoku 4:e00e709d7173 194 } else if (val=='c') {
hiromasaoku 4:e00e709d7173 195 stringData[indexStringData] = 0 ;
hiromasaoku 4:e00e709d7173 196 int power=atoi(stringData);
hiromasaoku 4:e00e709d7173 197 indexStringData=0;
hiromasaoku 4:e00e709d7173 198 IO.setRGBPower(power);
hiromasaoku 4:e00e709d7173 199 }
hiromasaoku 4:e00e709d7173 200
hiromasaoku 4:e00e709d7173 201
hiromasaoku 4:e00e709d7173 202 }
mbedalvaro 0:4e12dea53fbe 203 }