Diff: main.cpp

Revision:

0:ca94aa9bf823

Child:

1:dafb63606b66

diff -r 000000000000 -r ca94aa9bf823 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 13 17:46:45 2015 +0000
@@ -0,0 +1,191 @@
+//****************************************************************************/
+// Includes
+#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
+#include "HIDScope.h"
+#include "biquadFilter.h"
+#include "inits.h" // all globals, pin and variable initialization 
+#include "EMG.h"
+
+void setGoFlag(){
+    if (goFlag==true){
+        //pc.printf("rate too high, error in setGoFlag \n\r");
+        // flag is already set true: code too slow or frequency too high    
+    }
+    goFlag=true;
+}
+
+void systemStart(){
+    systemOn=true;
+}
+void systemStop(){
+    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)
+    initMotors();
+    initPIDs();
+    
+    outofboundsled=1;
+    edgeleft.mode(PullUp);
+    edgeright.mode(PullUp);
+    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){
+            /*********** Contents *************/
+            // 1) get emg signal
+            // 2) calculate desired angle velocities
+            // 3) calculate current angle velocities
+            // 4) pid controller
+            // 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
+            if (request_pos > request_neg){
+                request = request_pos; 
+            } 
+            else {
+                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;
+            rightAngle=(rightQei.getPulses()/round)*360+45;
+            
+            currentX = (tan(rightAngle*M_PI/180)*l)/(tan(leftAngle*M_PI/180)+tan(rightAngle*M_PI/180));
+            currentY = tan(leftAngle*M_PI/180)*currentX;
+            
+            // restrict motion if edges are touched
+            if (edgeleft==0){
+                if (request < 0){request=0; pc.printf("hit left edge \n\r");}
+            }
+            if (edgeright==0){
+                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
+            toX=currentX+request*RATE; 
+            toY=currentY+0*RATE; // should be vertical request*RATE
+            
+            toLeftAngle = atan(toY/toX)*180/M_PI;
+            toRightAngle = atan(toY/(l-toX))*180/M_PI;
+            
+            // restrict motion if angles out-of-bound
+            if (toLeftAngle < 0){toLeftAngle=0; pc.printf("out of bounds \n\r");}
+            if (toLeftAngle > 90){toLeftAngle=90; pc.printf("out of bounds \n\r");}
+            if (toRightAngle < 0){toRightAngle=0; pc.printf("out of bounds \n\r");}
+            if (toRightAngle > 90){toRightAngle=90; pc.printf("out of bounds \n\r");}
+            
+            //if (sqrt(pow(currentX,2)+pow(currentY,2)) > armlength){ // too far from left arm
+                //return 0;
+            //}
+            //if (sqrt(pow(l-currentX,2)+pow(currentY,2)) > armlength){ // too far from right arm
+                //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
+            
+            // 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
+            //leftVelocity = leftVelocityfilter.step(leftVelocity);
+            leftPrevPulses = leftPulses;
+            leftController.setProcessValue(leftVelocity);
+            
+            // right
+            rightPulses = rightQei.getPulses()/round*360;
+            rightVelocity = ((float)(rightPulses - rightPrevPulses))/ RATE;
+            rightVelocity = rightVelocity; // scale to 0 - 1, max velocity = 4200
+            //rightVelocity = rightVelocityfilter.step(rightVelocity);
+            rightPrevPulses = rightPulses;
+            rightController.setProcessValue(rightVelocity);
+            
+            
+            // ***********
+            // PID control output
+            // left
+            
+            leftPwmDuty = leftController.compute();
+            if (leftPwmDuty < 0){
+                leftDirection = 0;
+                leftMotor = -leftPwmDuty;
+            }
+            else {
+                leftDirection = 1;
+                leftMotor = leftPwmDuty;
+            }
+            
+            // right
+            rightPwmDuty = rightController.compute();
+            if (rightPwmDuty < 0){
+                rightDirection = 1;
+                rightMotor = -rightPwmDuty;
+            }
+            else {
+                rightDirection = 0;
+                rightMotor = rightPwmDuty;
+            }
+            
+            // User feedback
+            scope.set(0, leftRequest);
+            scope.set(1, leftPwmDuty);
+            scope.set(2, leftVelocity);
+            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)
+        {
+            leftMotor=0;
+            rightMotor=0;
+        }
+    }
+    
+}