Edit of Vector code

diff --git a/src/opcontrol.c b/src/opcontrol.c
index b455e210a6d890fb97723948e5c578e656f7ed87..d4d50a2cde262730906dd3f1769d63a6f6b36c20 100644 (file)
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -201,6 +201,8 @@ void operatorControl(){
                delay(10);      // Short delay to allow task switching.\r
        }\r
 }\r
+\r
+\r
 //Totally Theoretical PseudoCode for AutoShoot\r
 //Based on IME data:\r
 double lVel;//=velocity of left side\r
@@ -208,6 +210,7 @@ double rVel;//=velocity of right side
 double cVel;//=velocity of center (calculated below)\r
 double aVel;//=angular velocity (calculated below)\r
 \r
+\r
 //Used to find rectangular vectors * double aVel=angular velocity of robot\r
 //Used to store the rectangular vectors:\r
 double xVel1;\r
@@ -215,37 +218,71 @@ double xVel2;
 double yVel1;\r
 double yVel2;\r
 \r
-//Final Position Vectors\r
+\r
+//Final Position Vectors, robot and target\r
 double xPos;\r
 double yPos;\r
+double xTarget;\r
+double yTarget;\r
+\r
+\r
+//time difference variables\r
+double time1=0;\r
+double time2=0;\r
+\r
+int start;//1 is right blue, 2 is left blue, 3 is right blue, 4 is left blue\r
+\r
 \r
 //Vector Assignments:\r
 void Vectors(){\r
-while(1){//something in the brackets\r
-       int i;\r
-       i++;\r
-       cVel=(lVel+rVel)/2;\r
-       if(lVel>rVel){\r
-               aVel=cVel/(16*lVel/(rVel-lVel));\r
-       }\r
-       if(rVel>lVel){\r
-               aVel=cVel/(16*rVel/(lVel-rVel));\r
-       }\r
-       else{\r
-               aVel=0;\r
-                               if(i%2==0){\r
-                                       xVel1=cos(aVel)*cVel;\r
-                                       yVel1=sin(aVel)*cVel;\r
-                               }\r
-                               else{\r
-                                       xVel2=cos(aVel)*cVel;\r
-                                       yVel2=sin(aVel)*cVel;\r
-                               }\r
-\r
-               xPos+=((xVel1+xVel2)/2);//*time elapsed between cycles\r
-                               yPos+=((yVel1+yVel2)/2);//*time elapsed between cycles\r
-                               delay(20);\r
-       }\r
-}\r
-}\r
+       if(start==1){xPos=37;yPos=138;xTarget=137;yTarget=7;}\r
+       if(start==2){xPos=13;yPos=114;xTarget=137;yTarget=7;}\r
+       if(start==3){xPos=37;yPos=13;xTarget=137;yTarget=137;}\r
+       if(start==4){xPos=13;yPos=37;xTarget=137;yTarget=137;}\r
+       while(1){//something in the brackets\r
+\r
+               imeGetVelocity(DRIVEL,&lVel);//get reading of left velocity\r
+               lVel=lVel/24.5*8.64;\r
+\r
+               imeGetVelocity(DRIVER,&rVel);//get reading of right velocity\r
+               rVel=rVel/24.5*8.64;\r
+\r
+               int i=0;//counter for use of alternating variables\r
+               i++;\r
+               if(i==1){//just used the first time\r
+                       time1=milis();\r
+               }\r
+               cVel=(lVel+rVel)/2;//calculates the (c)enter of the robot's velocity\r
+\r
+               if(lVel>rVel){\r
+                       aVel=cVel/(16*lVel/(rVel-lVel));\r
+               }\r
+\r
+               if(rVel>lVel){\r
+                       aVel=cVel/(16*rVel/(lVel-rVel));\r
+               }\r
+\r
+               else{\r
+                       aVel=0;\r
+\r
+                       if(i%2==0){\r
+                               xVel1=cos(aVel)*cVel;//every 2 it uses these variables\r
+                               yVel1=sin(aVel)*cVel;\r
+                       }\r
+\r
+                       else{\r
+                               xVel2=cos(aVel)*cVel;//otherwise it uses these\r
+                               yVel2=sin(aVel)*cVel;\r
+                       }\r
+\r
+                       time2=milis();//records the time before calculating\r
+\r
+                       xPos+=((xVel1+xVel2)/2)*(time2-time1);//finds the area of the x curve using trapezoidal approx.\r
+                       yPos+=((yVel1+yVel2)/2)*(time2-time1);//finds the area of the y curve using trapezoidal approx.\r
+\r
+                       delay(20);\r
+               }\r
+               time1=milis();//records time between calculations for next time\r
+       }//end of while\r
+}//end of Vectors\r
 \r