Diff: turret.cpp

Revision:

0:72ae4ee22e56

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/turret.cpp	Mon May 09 23:51:59 2016 +0000
@@ -0,0 +1,110 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "QEI.h"
+#define pi (3.14159265)
+#define logfreq (100)
+#define data_pts (500)
+Serial pc(USBTX, USBRX);
+Motor m(p25, p23, p27);
+
+QEI enc(p16,p17,NC,1600);
+
+
+
+float motspeed;
+float x;
+float rad;
+int count;
+Timer t;
+float rad_one=0;
+float time_one=0;
+float omega;
+float old_E;
+float E;
+float PI=3.14159265;
+float old_t;
+float DC;
+float old_DC;
+float theta;
+//float theta_desired=x;
+float Ts=0.01;
+
+int main()
+{
+    t.reset();
+    enc.reset();
+    pc.printf("enter an angle\r\n");
+    pc.scanf("%f", &x);
+
+
+
+
+    while(1) {
+        t.start();
+        wait(1/logfreq);
+        //if(t.read()-old_t>=Ts)   
+
+        count = enc.getPulses() ;
+        theta = -count*((2*pi)/3200) ;
+        // Calculate position error
+        E=(x*(PI/180))-theta;
+        
+            // Control algorithm
+        DC = ((0.9841*old_DC) + (0.2348*E) - (0.2335*old_E));
+        old_E  = E;
+        m.speed(DC+0.15);
+        old_DC = DC;
+        
+        if(DC>0 && DC<0.7)
+        m.speed(DC+0.1);
+        if(E>-1.0)
+        m.speed(DC+0.15);
+        if (E<0.015 && E>-0.01)
+        m.speed(0.0);
+        else if(DC<0 && DC>-0.7){
+        m.speed(DC-0.34);
+        if(E>-1.0)
+        m.speed(DC-0.5);
+        if (E>-0.015&& E<0.01)
+        m.speed(0.0);}
+       
+        // Age variables
+        
+        
+        old_t  = t.read();
+
+
+        //omega=(rad-rad_one)/(t-time_one);
+        //right is negative, left is positive theta
+        
+        //if (x<0){
+        //  m.speed(0.25);  
+        //if(rad==x){
+        //m.speed(0);}}
+        //if (x>0){
+        // m.speed(-0.5);
+        //  if(rad==x){
+        //m.speed(0);}}
+        //rad_one=rad;
+        //time_one=t; 
+        printf("time: %f  angle: %f  error: %f  DC: %f\n\r", t.read(), theta, E,DC);
+    }
+
+
+    //0.2 for forward torque
+    //-0.5 for backward
+
+
+    //for (servopos=0; servopos<=1.0; servopos+=0.1){
+    //for (motspeed= 0.2; motspeed <= 1.0; motspeed += 0.1) {
+    //myservo1 = servopos;
+    //myservo2 = 1.0 - servopos;
+    //printf("Servo 1 position %.2f  Servo 2 position %.3f\n", servopos, (1-servopos));
+
+    //m.speed(motspeed);
+    //printf("motor speed is %.2f\n", motspeed);
+    //wait(0.0002);
+    //m.speed(-1.0);
+    //wait(0.002);
+    //m.speed(1.0);
+}