Diff: main.cpp

Revision:

14:314b86828ed2

Parent:

13:26263959903b

Child:

15:1d931a305464

--- a/main.cpp	Fri Apr 05 07:13:33 2013 +0000
+++ b/main.cpp	Fri Apr 05 08:21:46 2013 +0000
@@ -19,7 +19,7 @@
 float sint=0, cost=1;
           
 float vx = 0, vy=0;
-float theta=0, dt_betWords=50000, st=400;
+float theta=0, dt_betWords=50000, st=1800;
 unsigned int dt=500, ticktime=600;
 
 bool newPositionReady=false;
@@ -33,9 +33,9 @@
     int laserSwitch;   // laser {1: on 0:off} until next step
 };
 
-point2dl shearing(point2dl dataBefore, float radious, float angularSpeed){
+point2dl shearing(point2dl dataBefore, float velocity){
     point2dl dataAfter;
-    dataAfter.x = dataBefore.x - radious*angularSpeed*timer_v.read_us()/st/1000;   //st is a parameter for ajustment
+    dataAfter.x = dataBefore.x + velocity*timer_v.read_us()/st/1000;   //st is a parameter for ajustment
     dataAfter.y = dataBefore.y;
     dataAfter.laserSwitch = dataBefore.laserSwitch;
     
@@ -79,7 +79,7 @@
 
 void computeSpeed() {
     // We know exactly how much time passed since we last computed the speed, this is PERIODIC_COMPUTE in microseconds
-    angularSpeed = ( 1000000.0 * (float)(angle-oldAngle) / (float)(PERIODIC_COMPUTE) + angularSpeed)/2; // in rad/sec
+    angularSpeed =  1000000.0 * (float)(angle-oldAngle) / (float)(PERIODIC_COMPUTE); // in rad/sec
     oldAngle=angle;
 }
    
@@ -154,8 +154,10 @@
             
                 if (pc.readable()>0) processSerial();
                 
-                point2dl sheared = shearing(libletter[inputletters[i]-'a'].letpoints[j] , radious , angularSpeed );
-                point2dl rotated = rotation(sheared, angle );
+                point2dl sheared = shearing(libletter[inputletters[i]-'a'].letpoints[j] , radious*angularSpeed );
+                point2dl rotated = rotation(sheared, angle);
+                X = CENTER_AD_MIRROR_X + radious * cost;
+                Y = CENTER_AD_MIRROR_Y + radious * sint;
                 IO.writeOutXY(X + rotated.x,Y + rotated.y );
                 
                 /*