Diff: main.cpp

Revision:

29:aa8fe9fccec8

Parent:

20:8475768fc2f7

--- 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);