Diff: main.cpp

Revision:

1:dafb63606b66

Parent:

0:ca94aa9bf823

Child:

3:70f78cfc0f25

--- a/main.cpp	Tue Oct 13 17:46:45 2015 +0000
+++ b/main.cpp	Tue Oct 13 18:06:20 2015 +0000
@@ -23,14 +23,7 @@
     systemOn=false;
     pc.printf("system stopped\n\r");
 }
-void stoptop(){
-    pc.printf("stop top\n\r");
-}
-void gotop(){
-    pc.printf("go top\n\r");
-    
-}
-bool skipvelocity=false;
+
 int main() {
     
     // initialize (defined in inits.h)
@@ -40,17 +33,13 @@
     outofboundsled=1;
     edgeleft.mode(PullUp);
     edgeright.mode(PullUp);
+    
+    // interrupts
     motorControlTicker.attach(&setGoFlag, RATE);
-    //T1.attach(&samplego, (float)1/Fs);
-    
     cali_button.rise(&calibrate);
     startButton.rise(&systemStart);
     stopButton.rise(&systemStop);
-    //edgetop.rise(&stoptop);
-    //edgetop.fall(&gotop);
     
-    
-    endTimer.start(); //Run for 100 seconds.
     while (true){
         
         if (goFlag==true && systemOn==true){
@@ -58,17 +47,17 @@
             // 1) get emg signal
             // 2) calculate desired angle velocities
             // 3) calculate current angle velocities
-            // 4) pid controller
+            // 4) pid controller output
             // 5) user output
                 
-            
+            // **************
             // get emg signal
             sample_filter();
                        
             request_pos=y1;
             request_neg=y2;
             
-            // determine if forward or backward signal is stronger and set reference
+            // determine if forward or backward signal is stronger
             if (request_pos > request_neg){
                 request = request_pos; 
             } 
@@ -76,13 +65,14 @@
                 request = -request_neg;
             }
             request=request*maxspeed; // turn [-1, 1] into [-max cm/s, max cm/s]
-                     
+            
+            // ***************       
             // calculate required rotational velocity from the requested horizontal velocity
-            // first get the current position from the motor encoders
-            // make them start at 45 degree.
-            leftAngle=(leftQei.getPulses()/round)*360+45;
+            // first get the current position from the motor encoders and make them start at 45 degree. 
+            leftAngle=(leftQei.getPulses()/round)*360+45; 
             rightAngle=(rightQei.getPulses()/round)*360+45;
             
+            // trigonometry to get xy position from angles (cm)
             currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
             currentY = tan(leftAngle*M_PI/180)*currentX;
             
@@ -94,10 +84,11 @@
                 if (request > 0){request=0; pc.printf("hit right edge \n\r");}
             }
             
-            // calculate the position to go to according the the current position + the distance that should be covered in this timestep
+            // calculate the position to go to according the the current position + the distance that should be covered in this timestep (cm)
             toX=currentX+request*RATE; 
             toY=currentY+0*RATE; // should be vertical request*RATE
             
+            // trigonometry to get angles from xy new position (degree)
             toLeftAngle = atan(toY/toX)*180/M_PI;
             toRightAngle = atan(toY/(l-toX))*180/M_PI;
             
@@ -107,6 +98,7 @@
             if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
             if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
             
+            // restrict motion if position is out of reach of the arms
             //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
                 //return 0;
             //}
@@ -114,43 +106,37 @@
                 //return 0;
             //}
             
-            // calculate how much the angles should change in this timestep
-            leftDeltaAngle=(toLeftAngle-leftAngle);
-            rightDeltaAngle=(toRightAngle-rightAngle);
-            
-            // calculate the neccesairy velocities to make these angles happen.
-            leftRequest=(leftDeltaAngle/RATE); // degrees/sec
-            rightRequest=(rightDeltaAngle/RATE); // degrees/sec
+            // calculate the neccesairy velocities to make these angles happen (degree/sec)
+            leftRequest=(toLeftAngle-leftAngle)/RATE;
+            rightRequest=(toRightAngle-rightAngle)/RATE;
             
             // set the setpoint to the pid controller
             leftController.setSetPoint(leftRequest);
             rightController.setSetPoint(rightRequest);
             
-            // *******************
+            // **************
             // Velocity calculation
-            // left
-            leftPulses = leftQei.getPulses()/round*360; // in degree
-            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; //degree /sec
-            leftVelocity = leftVelocity; // scale to 0 - 1, max velocity = 20 degrees /sec
+            // left, differentiate from encoders
+            leftPulses = leftQei.getPulses()/round*360; // (degree)
+            leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE; // (degree/sec)
             //leftVelocity = leftVelocityfilter.step(leftVelocity);
             leftPrevPulses = leftPulses;
-            leftController.setProcessValue(leftVelocity);
+            leftController.setProcessValue(leftVelocity); // set PID process value
             
             // right
-            rightPulses = rightQei.getPulses()/round*360;
-            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
-            rightVelocity = rightVelocity; // scale to 0 - 1, max velocity = 4200
+            rightPulses = rightQei.getPulses()/round*360; // (degree)
+            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE; // (degree/sec)
             //rightVelocity = rightVelocityfilter.step(rightVelocity);
             rightPrevPulses = rightPulses;
-            rightController.setProcessValue(rightVelocity);
+            rightController.setProcessValue(rightVelocity); // set PID process value
             
             
-            // ***********
+            // **************
             // PID control output
             // left
             
             leftPwmDuty = leftController.compute();
-            if (leftPwmDuty < 0){
+            if (leftPwmDuty < 0){ // put motor in reverse when we have a negative value
                 leftDirection = 0;
                 leftMotor = -leftPwmDuty;
             }
@@ -161,7 +147,7 @@
             
             // right
             rightPwmDuty = rightController.compute();
-            if (rightPwmDuty < 0){
+            if (rightPwmDuty < 0){ // put motor in reverse when we have a negative value
                 rightDirection = 1;
                 rightMotor = -rightPwmDuty;
             }
@@ -170,6 +156,7 @@
                 rightMotor = rightPwmDuty;
             }
             
+            // ***************
             // User feedback
             scope.set(0, leftRequest);
             scope.set(1, leftPwmDuty);
@@ -177,8 +164,7 @@
             scope.set(3, currentX);
             scope.set(4, currentY);
             scope.send();
-            //pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
-        
+                    
             goFlag=false;
         }
         if(systemOn==false)