Positioning Attempt

This might work. Clyne should adapt and maybe use tomorrow

diff --git a/Nate's Position Testing/Shooter Testing/src/opcontrol.c b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
index 3ac3d9bf7c0f1ea0600753cc2ac2b478728a648a..df21076707e1004c82aa7318d511930f552e68dc 100644 (file)
--- a/Nate's Position Testing/Shooter Testing/src/opcontrol.c
+++ b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
@@ -1,6 +1,7 @@
 #include "main.h"\r
 \r
 #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))\r
+#define PI =3.14159\r
 /*\r
  * Runs the user operator control code. This function will be started in its own task with the\r
  * default priority and stack size whenever the robot is enabled via the Field Management System\r
@@ -18,29 +19,8 @@
  *\r
  * This task should never exit; it should end with some kind of infinite loop, even if empty.\r
  *\r
-lDist*r=theta\r
-rDist*(r+15)=theta\r
-cDist*(r+7.5)=theta\r
-theta/lDist=r\r
-rDist*(theta/lDist+15)=theta\r
-rDist*theta/lDist+15*rDist=theta\r
-rDist/lDist+15*rDist/theta=1\r
-1-15*rDist/theta=rDist/lDist\r
-theta=(1-15*rDist)*lDist/rDist\r
-theta=(lDist-15*rDist*lDist)/rDist\r
-theta=lDist/rDist-heading*lDist\r
-theta=\r
 */\r
-static int             rRot,lRot,cRot,time1,time2,iHeading;\r
-static float   lDist,\r
-                               theta,\r
-                               rDist,\r
-                               cDist,\r
-                               wheelD=3,\r
-                               pi=3.14159,\r
-                               heading=15,\r
-                               startTheta,\r
-                               fHeading;\r
+\r
 /* notes for Clyne:\r
  * Other processes should not interupt the collection of data:\r
  * rotation calculation should be and data collection should be a high priority/primary task\r
@@ -51,42 +31,20 @@ static float        lDist,
  *\r
  *\r
  */\r
+static long lVel,rVel,deltaPos,deltaTime,startTheta,\r
+                                         heading,lDist,rDist,xPos,yPos,\r
+                                         track=15;\r
 void operatorControl() {\r
        heading=startTheta;\r
        while (1) {\r
-               time1=micros();\r
-               cRot=(encoderGet(encoder1)-encoderGet(encoder2));\r
-               cDist=(cRot*pi)/120;\r
-               fHeading=(cDist/12)*(360/(2*pi));\r
-               iHeading=round(fHeading)%360;\r
-\r
-               lcdPrint(uart2,1,"iHeading: %d",iHeading);\r
-\r
-               if(fHeading<360){\r
-                       motorSet(10,-60);\r
-                       motorSet(1,20);\r
-\r
-               }else{\r
-                       motorSet(10,0);\r
-                       motorSet(1,0);\r
 \r
-               }\r
-               /* lRot=encoderGet(encoder1);\r
-               rRot=encoderGet(encoder2);\r
-               lDist=(lRot*pi)/120;\r
-               rDist=(rRot*pi)/120;\r
-               lcdPrint(uart2,1,"iHeading: %d",iHeading);\r
-               if(digitalRead(1)==0){\r
-               motorSet(1,120);\r
-               }\r
-               else if(digitalRead(2)==0){\r
-               motorSet(10,-120);\r
-               }\r
-               else{\r
-               motorSet(1,0);\r
-               motorSet(10,0);\r
-               }\r
-               */\r
+               //get time 1 the first time\r
+               heading=((lDist-rDist)/(track*2*PI))*360+startTheta;//heading in degrees. might need to be radians for cosine functions\r
+               //get time 2\r
+               deltaPos=(lVel+rVel)/2*deltaTime;\r
+               xPos+=cos(heading)*deltaPos;\r
+               yPos+=sin(heading)*deltaPos;\r
+               //get time 1\r
                delay(20);\r
 \r
        }\r

diff --git a/bin/auto.o b/bin/auto.o
new file mode 100644 (file)
index 0000000..d06291a
Binary files /dev/null and b/bin/auto.o differ

diff --git a/bin/init.o b/bin/init.o
new file mode 100644 (file)
index 0000000..eebfb38
Binary files /dev/null and b/bin/init.o differ

diff --git a/bin/opcontrol.o b/bin/opcontrol.o
new file mode 100644 (file)
index 0000000..8d29c81
Binary files /dev/null and b/bin/opcontrol.o differ

diff --git a/bin/output.bin b/bin/output.bin
new file mode 100755 (executable)
index 0000000..1eef409
Binary files /dev/null and b/bin/output.bin differ

diff --git a/bin/zephyr.o b/bin/zephyr.o
new file mode 100644 (file)
index 0000000..c9daf0a
Binary files /dev/null and b/bin/zephyr.o differ

diff --git a/src/auto.c b/src/auto.c
index 23324284dd2e0b8b3de18b522351ac6ff0374c29..098412e168f5ed79eb6e231ca3dad9677e67eb09 100644 (file)
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,4 +1,5 @@
 #include <main.h>\r
+#include <zephyr.c>\r
 \r
 /*#define TARGET_SPEED 65\r
 \r

diff --git a/src/zephyr.c b/src/zephyr.c
index 7bfd76ed0f3c144dd3245029650d5c1cd7c64560..109742f7e0ce4c9fe21ea0da8eff48dd52817626 100644 (file)
--- a/src/zephyr.c
+++ b/src/zephyr.c
@@ -1,6 +1,8 @@
 #include <zephyr.h>
 #include <main.h>
 
+#define PI =3.14159
+
 #include <string.h>
 
 #ifdef LCD_PORT