Diff: main.cpp

Revision:

12:d945fb6d4988

Parent:

11:8a0fecc86705

Child:

13:26263959903b

--- a/main.cpp	Fri Mar 29 08:55:10 2013 +0000
+++ b/main.cpp	Fri Apr 05 04:28:08 2013 +0000
@@ -28,13 +28,34 @@
 
 bool start=false;
 
-struct point2d {
+struct point2dl {
     int x,y;
     int laserSwitch;   // laser {1: on 0:off} until next step
 };
+
+point2dl shearing(point2dl dataBefore, float radious, float angularSpeed){
+    point2dl dataAfter;
+    dataAfter.x = dataBefore.x - radious*angularSpeed*timer_v.read_us()/st/1000;
+    dataAfter.y = dataBefore.y - radious*angularSpeed*timer_v.read_us()/st/1000;
+    dataAfter.laserSwitch = dataBefore.laserSwitch;
+    
+    return dataAfter;
+}
+
+point2dl rotation(point2dl dataBefore, float theta){
+    sint = sin(theta);
+    cost = cos(theta);
+    point2dl dataAfter;
+    dataAfter.x = sint*dataBefore.x + cost*dataBefore.y;
+    dataAfter.y = -cost*dataBefore.x + sint*dataBefore.y;
+    dataAfter.laserSwitch = dataBefore.laserSwitch;
+
+    return dataAfter;
+}
+
 struct letter {
     int pointnum;
-    vector<point2d> letpoints;
+    vector<point2dl> letpoints;
 };
 
 vector<letter> libletter;   // letter library
@@ -78,7 +99,7 @@
         letter bufl;
         fscanf(fp, "%d", &bufl.pointnum);
         for(int j=0; j<bufl.pointnum; j++) {
-            point2d bufp;
+            point2dl bufp;
             fscanf(fp, "%d", &bufp.x);
             fscanf(fp, "%d", &bufp.y);
             fscanf(fp, "%d", &bufp.laserSwitch);
@@ -132,16 +153,21 @@
             for(int j=0; j<libletter[inputletters[i]-'a'].letpoints.size(); j++) {
             
                 if (pc.readable()>0) processSerial();
-                sint = sin(angle);
-                cost = cos(angle);
                 
-                int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - radious*angularSpeed*timer_v.read_us()/st*j/1000); //this ZURE should be modifyed not by dt but Timer.
+                point2dl sheared = shearing(libletter[inputletters[i]-'a'].letpoints[j] , radious , angularSpeed );
+                point2dl rotated = rotation(sheared, angle );
+                IO.writeOutXY(X + rotated.x,Y + rotated.y );
+                
+                /*
+                int x = -0.8*((libletter[inputletters[i]-'a'].letpoints[j].x ) - radious*angularSpeed*timer_v.read_us()/st/1000); //this ZURE should be modifyed not by dt but Timer.
                 int y = (libletter[inputletters[i]-'a'].letpoints[j].y);
                 IO.writeOutXY(X + radious*cost + sint*x + cost*y,Y + radious*sint - cost*x + sint*y);
-                timer_v.reset();
+                */
+                
                 wait_us(dt);
                 IO.setRedPower(libletter[inputletters[i]-'a'].letpoints[j].laserSwitch);//on
             }
+            timer_v.reset();
             IO.setRedPower(0); //off
             wait_us(dt_betwords);//sqrtf(vx*vx+vy*vy)*100000);