Revision 29:aa8fe9fccec8, committed 2013-07-30

Comitter:

takapiasano

Date:

Tue Jul 30 04:16:21 2013 +0000

Parent:

20:8475768fc2f7

Commit message:

close to complete

Changed in this revision

--- a/main.cpp	Fri May 31 06:32:07 2013 +0000
+++ b/main.cpp	Tue Jul 30 04:16:21 2013 +0000
@@ -9,8 +9,13 @@
 InterruptIn clockEncoderPin(p14);   //blue
 DigitalIn directionPin(p15);    //green
 
+AnalogIn iny(p20);
+AnalogIn inx(p19);
+AnalogIn inPotX(p16);
+AnalogIn inPotY(p17);
+
 void processSerial();
-Timer timer_v;          // for shearing        //for send the speed on the serial port every 30 ms:
+//Timer timer_v;          // for shearing        //for send the speed on the serial port every 30 ms:
 LocalFileSystem local("local");
 //Serial pc(USBTX, USBRX);
 
@@ -23,7 +28,9 @@
 
 float vx = 0, vy=0;
 float theta=0, dt_betWords=50000, st=1800;
-unsigned int dt=500, ticktime=600;
+int laserColor=1, ticktime=600;
+int potxx = 0,potyy = 0;
+
 
 bool newPositionReady=false;
 unsigned int counter=0;
@@ -49,31 +56,54 @@
 */
 
 ////for culculate rotary velocity --------------------------------------------------------
-
-volatile float angleIncrement = 2.0*PI / 1024; // when in Sign/Magnitude mode
-float radious = 700, attachSecond = 10000;
+int radious = 700, attachSecond = 500;
+/*volatile float*/ int  angleIncrement = 1;//2.0*PI / 2500;// when in Sign/Magnitude mode
 
 // a ticker function to compute the speed periodically-------------------------------------------------------------
-#define PERIODIC_COMPUTE 10000 // in us
+#define PERIODIC_COMPUTE 5000 // in us
 volatile float angularSpeed = 0;
-volatile float angle=0, oldAngle=0;
+float angleX=0.5;
+float angleY=0.5;
+float angleSpeedX=0;
+float angleSpeedY=0;
 Ticker speedTimerCompute;
 Ticker superEncoder;
 
+float PotX, PotY;
+
+
 // the external interrupt routine:
 void encoderClock()
 {
-    if (directionPin) angle += angleIncrement ;
-    else angle -= angleIncrement;
+    pc.printf("%f,%f,\n % f, %f \n\n" , PotX,PotY,angleX,angleY);
 }
 
 void computeSpeed()
 {
-    // We know exactly how much time passed since we last computed the speed, this is PERIODIC_COMPUTE in microseconds
-    float oldspeed = angularSpeed;
-    angularSpeed = ( 1000000.0 * (float)(angle-oldAngle) / (float)(PERIODIC_COMPUTE) + oldspeed )/ 2.0; // in rad/sec
-    oldAngle=angle;
-    myRender.updateSpeed(angularSpeed,angle);
+    float oldAngleX = angleX;
+    float oldAngleY = angleY;
+    
+    angleX = inx;
+    angleY = iny;
+    
+    PotX = inPotX;//0.25*potxx;//
+    PotY = inPotY;//0.25*potyy;//
+    
+    // low pass filter (recusive filter):
+    float alpha=.35; // alpha=dt/(RC+dt), with dt=COMPUTE_SPEED, and f_cut=1/(2*PI*RC) is the filter cutoff frequency
+                    // we want f_cut=1kHz, alpha=COMPUTE_SPEED/(1/(2.PI.fcut)+COMPUTE_SPEED) => alpha=31.415
+    angleX=alpha*angleX+(1-alpha)*oldAngleX;
+    angleY=alpha*angleY+(1-alpha)*oldAngleY;
+    
+    if(abs(angleX - oldAngleX) <0.0005 || abs(angleY - oldAngleY) < 0.0005) myRender.startSwitch=1; 
+    
+    angleSpeedX = -(angleX - oldAngleX) * PotX;
+    angleSpeedY = -(angleY - oldAngleY) * PotY;
+    
+    //pc.printf("%f %f \n" ,angleX,angleY);
+    //pc.printf("%f %f \n", PotX,PotY);
+    
+    myRender.updateSpeed(angleSpeedX,angleSpeedY);
     void processSerial();
 }
 
@@ -107,10 +137,10 @@
     IO.init(); // note: serial speed can be changed by checking in the hardwareIO.cpp initialization
 
     // initialize the angle (arbitrary origin):
-    oldAngle=angle=0;
+    //oldAngle=angle=0;
 
     // Attach the external interrupt routine----------------------------------------------------------------------
-    superEncoder.attach_us(&encoderClock, 111);
+    //superEncoder.attach_us(&encoderClock, 500);
     //clockEncoderPin.rise(&encoderClock);
     //clockEncoderPin.fall(&encoderClock);
 
@@ -120,15 +150,18 @@
     myRender.startRender();
 
     // Attach the periodic computing function:
+    // We need to initialize the angleX and Y:
+     angleX = inx;
+    angleY = iny;
     speedTimerCompute.attach_us(&computeSpeed, PERIODIC_COMPUTE);
 
     // Set displaying laser powers--------------------------------------------------------------------------
-    IO.setRedPower(0);
+    IO.setRGBPower(0);
     IO.setGreenPower(0);
     wait_ms(100);
 
 
-    timer_v.start();
+    //timer_v.start();
     // MAIN LOOP: --------------------------------------------------------------------------------------------
     while(1) {
         if (pc.readable()>0) processSerial();
@@ -169,34 +202,21 @@
             else if (val == 'X') {
                 beforeX = X;
                 stringData[indexStringData] = 0;
-                X = atoi(stringData);
+                potxx = atoi(stringData);
                 indexStringData=0;
-                vx = ((float)X-(float)beforeX) / (float)timer_v.read_us() *1000;
-            }
-
-            else if (val == 'Y') {
+            } else if (val == 'Y') {
                 beforeY = Y;
                 stringData[indexStringData] = 0;
-                Y = atoi(stringData);
+                potyy = atoi(stringData);
                 indexStringData=0;
-                //timer_v.stop();
-                //newSpeedReady = true;
-                //if( (Y-beforeY) > 5){
-                vy = ((float)Y-(float)beforeY) / (float)timer_v.read_us() *1000;
-                
-                myRender.updateSpeed(vx, vy);
-                
-               
-            }
-
-            else if (val == 'D') {
+            } else if (val == 'R') {
                 stringData[indexStringData] = 0;
-                dt = atoi(stringData);
+                myRender.laserColor = 2;
                 indexStringData=0;
                 //makeBuffer();
-            } else if (val == 'B') {
+            } else if (val == 'G') {
                 stringData[indexStringData] = 0;
-                dt_betWords = atoi(stringData);
+                myRender.laserColor = 3;
                 indexStringData=0;
                 //makeBuffer();
             } else if (val == 'S') {
@@ -205,17 +225,26 @@
                 pc.printf("shearingSwitch : %d \n" , myRender.shearingSwitch);
                 indexStringData=0;
                 //makeBuffer();
-            } else if (val == 'R') {
+            } else if (val == 'A') {
                 stringData[indexStringData] = 0;
-                radious = atoi(stringData);
+                myRender.radious = atoi(stringData);
+                pc.printf("st : %d \n" , angleIncrement);
+                superEncoder.detach();
+                superEncoder.attach_us(&encoderClock, attachSecond);
                 indexStringData=0;
                 //makeBuffer();
             } else if (val == 'T') {
                 stringData[indexStringData] = 0;
                 attachSecond = atoi(stringData);
-                pc.printf("attachSecond : %d \n" , attachSecond);
+                encoderClock();
+                myRender.startSwitch = 1;
+                /*pc.printf("attachSecond : %d \n" , attachSecond);
+                
+                superEncoder.detach();
+                superEncoder.attach_us(&encoderClock, attachSecond);
                 indexStringData=0;
                 //makeBuffer();
+                */
             }
             // X value?
             /*else if (val=='x') {
@@ -242,7 +271,7 @@
                 stringData[indexStringData] = 0 ;
                 int power=atoi(stringData);
                 indexStringData=0;
-                IO.setRedPower(power);
+                IO.setRGBPower(power);
             } else if (val=='c') {
                 stringData[indexStringData] = 0 ;
                 int power=atoi(stringData);

--- a/renderclass.cpp	Fri May 31 06:32:07 2013 +0000
+++ b/renderclass.cpp	Tue Jul 30 04:16:21 2013 +0000
@@ -9,15 +9,16 @@
 
 inline void render::shearing(point2dl& dataBefore)
 {
-    dataBefore.x = dataBefore.x - speed * cos(angle+PI/2)*speedTimer.read_us()/400;
-    dataBefore.y = dataBefore.y - speed * sin(angle+PI/2)*speedTimer.read_us()/400;
+    dataBefore.x = -dataBefore.x/2.0 - 10.0*intvx; //vx * speedTimer.read_us()*10; //2000000;//dataBefore.x - speed *speedTimer.read_us()/1000000 ;//- speed * cos(angle+PI/2)*speedTimer.read_us()/800;
+    dataBefore.y = dataBefore.y/2.0 - 10.0*intvy; //vy * speedTimer.read_us()*10;//dataBefore.y ;//- speed * sin(angle+PI/2)*speedTimer.read_us()/800;
 }
 
 inline void render::rotation(point2dl& dataBefore)
 {
     int x=dataBefore.x;
-    dataBefore.x = 1800 * cos(angle) + sin(angle)*x - cos(angle)*dataBefore.y;
-    dataBefore.y = 1800 * sin(angle) - cos(angle)*x - sin(angle)*dataBefore.y;
+    dataBefore.x = cos(angle)*x - sin(angle)*dataBefore.y;//
+    dataBefore.y = sin(angle)*x + cos(angle)*dataBefore.y;
+
 }
 //----------------------------------------------------------------------------------
 
@@ -26,94 +27,175 @@
     speedTimer.start();
     renderTimer.attach_us(this, &render::draw,RENDER_INTERVAL);
     pc.printf("render started!\n");
+    pc.printf("int : %f, %f,   v :, %f,%f, " , intvx , intvy , vx , vy);
+    
 }
 
 void render::stopRender()
 {
     renderTimer.detach();
-    translated.clear();
-    currentLetters=0;
-    currentPoints=0;
+    speedTimer.stop();
 }
 
 void render::setRender(vector<letter> *p)
 {
+    translated.clear();
+    radious = 200;
+       
     ptext = p;
     pc.printf("Size of text to render: %d/n", (*ptext).size() );
     //for(int i=0; i!=(*ptext).size(); i++){
+    int count=0;
     for(std::vector<letter>::iterator itr = (*ptext).begin(); itr != (*ptext).end(); ++itr) {
-        translated.push_back(*itr);
+        letter translatedLetter;
+        for(int i=0; i<(*itr).letpoints.size(); i++) {
+            point2dl tranPoint;
+            tranPoint.x=itr->letpoints[i].x+560*count;
+            tranPoint.y=-itr->letpoints[i].y;
+            tranPoint.laserSwitch=itr->letpoints[i].laserSwitch;
+            translatedLetter.letpoints.push_back(tranPoint);
+        
     }                                            //copy the liblary to the buffer.
-    
-    
+    translated.push_back(translatedLetter);
+    count++;
+}
+
+    shearingSwitch = 1;
+    radious = 1800;
+
     currentLetters=0;
-    currentPoints=0;
-    shearingSwitch = 1;
 
+    firstPointCounter=0;
+    lastPointCounter=0;
+
+    rendererState=FIRST_POINT;
 }
 
 
-void render::updateSpeed(float gspeed, float gangle){
-    speed = gspeed;
-    angle = gangle;
+void render::updateSpeed(float vvx, float vvy)
+{
+    vx = vvx;
+    vy = vvy;
     
-    //speed=sqrt(vx*vx+vy*vy);//*factorSpeed;
-    //angle=atan2(vy,vx);
+    //intvx += RENDER_INTERVAL*vx;
+    //intvy += RENDER_INTERVAL*vy;
+    
+    angle= 0;//(1/9.0)*PI ;//+0.3* atan( vy / vx ) * 0.7*angle;   atan2
 }
 
-int stopcounter=100;
 
 void render::draw()
 {
-    if(translated.size()>0){
-    if(stopcounter == 99) speedTimer.reset();
-    if(stopcounter==100){
+    /*
+    if(firstPointCounter < 3){
+        
+                IO.writeOutXY(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
+                if(laserColor % 2 == 0) IO.setRedPower(translated[currentLetters].letpoints[currentPoints].laserSwitch);
+                if(laserColor % 3 == 0) IO.setGreenPower(translated[currentLetters].letpoints[currentPoints].laserSwitch);
+                
+        firstPointCounter+=1;
+    }
+    else{
+        
+                IO.setRedPower(0);
+                IO.setGreenPower(0);
+        firstPointCounter = 0;
+    }
     
-    point2dl drawPoint=translated[currentLetters].letpoints[currentPoints];
-    transform(drawPoint); // transform it with the current speed and angle
-    //IO.setRedPower(drawPoint.laserSwitch);
-    IO.writeOutXY(CENTER_AD_MIRROR_X + (int)drawPoint.x , CENTER_AD_MIRROR_Y + (int)drawPoint.y);
+    
+    */
     
     
-    if(currentPoints>0) IO.setRedPower(translated[currentLetters].letpoints[currentPoints-1].laserSwitch);
-    else                IO.setRedPower(0);             
+    //if(abs(vx)<0.001 || abs(vy) < 0.001)startSwitch = 1;
+    
+    if(translated.size()>0 && startSwitch == 1) {
+
+        switch(rendererState) {
+
+            case NORMAL_POINT:
+                intvx += speedTimer.read_us()*vx;
+                intvy += speedTimer.read_us()*vy;
+                speedTimer.reset();
+            
+                point2dl drawPoint=translated[currentLetters].letpoints[currentPoints];
+                transform(drawPoint);               // transform it with the current speed and angle
+                IO.writeOutXY(CENTER_AD_MIRROR_X + (int)drawPoint.x , CENTER_AD_MIRROR_Y + (int)drawPoint.y);
+                if(laserColor % 2 == 0) IO.setRedPower(translated[currentLetters].letpoints[currentPoints].laserSwitch);
+                if(laserColor % 3 == 0) IO.setGreenPower(translated[currentLetters].letpoints[currentPoints].laserSwitch);
+
+                // move to the next point
+                currentPoints++;
+
+                //Should we change the state?
+                if(currentPoints >= translated[currentLetters].letpoints.size() ) {
+                    rendererState=AFTER_LAST_POINT;
+                }
+                break;
 
-    currentPoints++;
-    }
-    else stopcounter++;
-    
-    if(currentPoints >= translated[currentLetters].letpoints.size() ) {
-        if(stopcounter == 100) stopcounter = 0;
-        else if(stopcounter == 0){
-        renderTimer.detach();
+            case AFTER_LAST_POINT:
+                // just wait!
+                if (lastPointCounter<WAITING_LAST) {
+                IO.setRedPower(0);
+                IO.setGreenPower(0);
+                lastPointCounter++;
+                }
+                else {
+                    lastPointCounter=0;
+                    currentPoints = 0;
+                    // switch the laser off and move to the first point of the next letter:
+                    
+                    currentLetters++;
+                    if(currentLetters >= translated.size()) {
+                        currentLetters = 0;
+                        //startSwitch=0;
+                        intvx = intvy =0;
+                        
+                    
+                    }
+                    rendererState=FIRST_POINT;
+                    
+                    }
+                    break;
+
+                case FIRST_POINT:
+                    
+                    if (firstPointCounter<WAITING_FIRST) {
+                    firstPointCounter++;
+                    
+                    // show first point!
+                    point2dl drawPoint=translated[currentLetters].letpoints[0];
+                    
+                    if(currentLetters!=0){
+                    intvx += speedTimer.read_us()*vx;
+                    intvy += speedTimer.read_us()*vy;
+                    }
+                    
+                    speedTimer.reset();
+                    transform(drawPoint);               // transform it with the current speed and angle
+                    IO.writeOutXY(CENTER_AD_MIRROR_X + (int)drawPoint.x , CENTER_AD_MIRROR_Y + (int)drawPoint.y);
+                    
+                    
+                    
+                    //IO.setRedPower(translated[currentLetters].letpoints[currentPoints].laserSwitch);
+
+                    } else {
+                        firstPointCounter=0;
+                        currentPoints=1;
+                        
+                        intvx += speedTimer.read_us()*vx;
+                        intvy += speedTimer.read_us()*vy;
+                        speedTimer.reset();
+                        
+                        rendererState=NORMAL_POINT;
+                    }
+                    break;
+                }
         }
-        //wait_us(RENDER_INTERVAL);
-        else if(stopcounter == 1){
-        IO.setRedPower(0);
-        //wait_us(RENDER_INTERVAL);
-        currentLetters++;
-        currentPoints = 0;
-        renderTimer.attach_us(this, &render::draw,RENDER_INTERVAL);
-        speedTimer.reset();
-        }
-    }
-    if(currentLetters >= translated.size()) {
-        currentLetters = 0;
-        currentPoints = 0;
-        renderTimer.detach();
-        //wait_us(RENDER_INTERVAL);
-        IO.setRedPower(0);
-        //wait_us(RENDER_INTERVAL*200);
-        renderTimer.attach_us(this, &render::draw,RENDER_INTERVAL);
-        speedTimer.reset();
-        stopcounter = -200;
-    }
-    }
+    
 }
 
-void render::transform(point2dl& mypoint)
-{
-    rotation(mypoint);
-    if(shearingSwitch == 1) shearing(mypoint);
+    void render::transform(point2dl& mypoint) {
+        rotation(mypoint);
+        if(shearingSwitch == 1) shearing(mypoint);
 }
 

--- a/renderclass.h	Fri May 31 06:32:07 2013 +0000
+++ b/renderclass.h	Tue Jul 30 04:16:21 2013 +0000
@@ -2,7 +2,11 @@
 #include <vector>
 #define PI 3.14159
 
-#define RENDER_INTERVAL 700// in microseconds
+#define RENDER_INTERVAL 500// in microseconds
+#define WAITING_FIRST 1 // this means WAITING_FIRST number of interrupt cycles in case of the first point
+#define WAITING_LAST 1
+
+enum stateMachine{NORMAL_POINT, FIRST_POINT, AFTER_LAST_POINT};
 
 struct point2dl {
     int x,y;
@@ -10,7 +14,7 @@
 };
 
 struct letter {
-    int pointnum;
+    int pointnum; //not used for the time being
     vector<point2dl> letpoints;
 };
 
@@ -24,9 +28,13 @@
     void transform(point2dl& mypoint);
     void shearing(point2dl& dataBefore);
     void rotation(point2dl& dataBefore);
-    void updateSpeed(float vx, float vy);
+    void updateSpeed(float vvx, float vvy);
 
     int shearingSwitch;
+    int laserColor;
+    
+    int radious;
+    int startSwitch;
     
     private:
     vector<letter> *ptext;
@@ -35,10 +43,17 @@
     int currentPoints;
     point2dl drawPoint;
     
+    stateMachine rendererState;
+    int firstPointCounter, lastPointCounter;
+    
     Ticker renderTimer;
     Timer   speedTimer;
     
     float speed;
     float angle;
+    float vx;
+    float vy;
+    float intvx,intvy;
     
-};
\ No newline at end of file
+    
+};