Diff: main.cpp

Revision:

32:c940f6b6a6a0

Parent:

31:8646e853979f

Child:

33:1c7b498ded25

--- a/main.cpp	Wed Oct 28 11:49:29 2015 +0000
+++ b/main.cpp	Wed Oct 28 13:09:25 2015 +0000
@@ -100,11 +100,11 @@
 
 
 //////////////////////////////// POSITION AND ANGLE SHIZZLE
-const float safetymarginfield=0.075; //adjustable, tweak for maximum but safe range
+const float safetymarginfield=0.1; //adjustable, tweak for maximum but safe range
 const float mm_per_sec_emg=0.1;// move the pod 100 mm per sec if muscle is flexed
 const float y_start=0.145;//starting y position of the pod
 const float y_punch=0.473;// position to where there is punched
-const float timetoshoot=0.25;// time it can take to shoot
+const float timetoshoot=0.35;// time it can take to shoot
 const float timetogoback=1;// time it can take to go back after shooting
 
 float desired_position=0;
@@ -161,7 +161,7 @@
 
 ////////////////////////FUNCTIONS
 //gather data and send to scope
-void scopedata(float wanted_y)
+void scopedata(double wanted_y)
 {
     scope.set(0,desired_position-anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
     scope.set(1,wanted_y-anglepos.angletoposition_y(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses()));
@@ -228,14 +228,14 @@
 void valuechange()
 {
     mycontroller.STOP();
-    pc.printf("KP is now %f, enter new value\n",Kp_move);
-    pc.scanf("%f", &Kp_move);
+    pc.printf("KP shoot is now %f, enter new value\n",Kp_shoot);
+    pc.scanf("%f", &Kp_shoot);
 
-    pc.printf("KI is now %f, enter new value\n",Ki_move);
-    pc.scanf("%f", &Ki_move);
+    pc.printf("KI shoot is now %f, enter new value\n",Ki_shoot);
+    pc.scanf("%f", &Ki_shoot);
 
-    pc.printf("KD is now %f, enter new value\n",Kd_move);
-    pc.scanf("%f", &Kd_move);
+    pc.printf("KD shoot is now %f, enter new value\n",Kd_shoot);
+    pc.scanf("%f", &Kd_shoot);
 
     pc.printf("Factor of tau_p=1.0/(factor*controlfrequency) is now %f, enter new value\n",factor_taup);
     pc.scanf("%f", &factor_taup);
@@ -248,11 +248,11 @@
 void shoot()
 {
     ledgreen=1;
-    float time=0;
-    float stepsize=(y_punch-y_start)/(timetoshoot*control_frequency);
-    float y_during_punch=y_start;// set initial y position to start position
-
-    float x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
+    double time=0;
+    double stepsize=(0.5*PIE)/(timetoshoot*control_frequency);
+    double profile_angle=0;
+    double x_punch=anglepos.angletoposition(counttorad*encoder1.getPulses(),counttorad*encoder2.getPulses());
+    double y_during_punch=y_start;
     Timer shoottimer;
     shoottimer.reset();
     shoottimer.start();
@@ -260,12 +260,15 @@
     while (time<=timetoshoot) {
         ledblue=!ledblue;
 
-        y_during_punch+=stepsize; // add stepsize to y position
+        profile_angle+=stepsize; // add stepsize to angle
+        
+        y_during_punch=(y_punch-y_start)*(1-cos(profile_angle))+y_start; // calculate y position for shooting
+        
         if (y_during_punch>=y_punch) {//to check if y position is not bigger than y_punch for safety
             y_during_punch=y_punch;
         } else {
             y_during_punch=y_during_punch;
-        }
+        }        
 
         desired_angle1=anglepos.positiontoangle1(x_punch,y_during_punch);// calculate desired angles
         desired_angle2=anglepos.positiontoangle2(x_punch,y_during_punch);
@@ -284,11 +287,13 @@
     //back
     time=0;
     shoottimer.reset();
-    stepsize=(y_punch-y_start)/(timetogoback*control_frequency);
+    stepsize=(PIE)/(timetogoback*control_frequency);
+    profile_angle=0;
     while (time<=timetogoback) {
         ledblue=!ledblue;
 
-        y_during_punch-=stepsize; // add stepsize to y position
+        profile_angle+=stepsize; // add stepsize to y position
+       y_during_punch=0.5*(y_punch-y_start)*(1+cos(profile_angle))+y_start; // calculate y position for shooting
         if (y_during_punch<=y_start) {//to check if y position is not smaller than y_start for safety
             y_during_punch=y_start;
         } else {