Positioning Attempt

This might work. Clyne should adapt and maybe use tomorrow

8 files changed, 15 insertions, 54 deletions

diff --git a/Nate's Position Testing/Shooter Testing/src/opcontrol.c b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
index 3ac3d9b..df21076 100644
--- 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"

 

 #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))

+#define PI =3.14159

 /*

  * Runs the user operator control code. This function will be started in its own task with the

  * default priority and stack size whenever the robot is enabled via the Field Management System

@@ -18,29 +19,8 @@
  *

  * This task should never exit; it should end with some kind of infinite loop, even if empty.

  *

-lDist*r=theta

-rDist*(r+15)=theta

-cDist*(r+7.5)=theta

-theta/lDist=r

-rDist*(theta/lDist+15)=theta

-rDist*theta/lDist+15*rDist=theta

-rDist/lDist+15*rDist/theta=1

-1-15*rDist/theta=rDist/lDist

-theta=(1-15*rDist)*lDist/rDist

-theta=(lDist-15*rDist*lDist)/rDist

-theta=lDist/rDist-heading*lDist

-theta=

 */

-static int 		rRot,lRot,cRot,time1,time2,iHeading;

-static float 	lDist,

-				theta,

-				rDist,

-				cDist,

-				wheelD=3,

-				pi=3.14159,

-				heading=15,

-				startTheta,

-				fHeading;

+

 /* notes for Clyne:

  * Other processes should not interupt the collection of data:

  * rotation calculation should be and data collection should be a high priority/primary task

@@ -51,42 +31,20 @@ static float 	lDist,
  *

  *

  */

+static long lVel,rVel,deltaPos,deltaTime,startTheta,

+					  heading,lDist,rDist,xPos,yPos,

+					  track=15;

 void operatorControl() {

 	heading=startTheta;

 	while (1) {

-		time1=micros();

-		cRot=(encoderGet(encoder1)-encoderGet(encoder2));

-		cDist=(cRot*pi)/120;

-		fHeading=(cDist/12)*(360/(2*pi));

-		iHeading=round(fHeading)%360;

-

-		lcdPrint(uart2,1,"iHeading: %d",iHeading);

-

-		if(fHeading<360){

-			motorSet(10,-60);

-			motorSet(1,20);

-

-		}else{

-			motorSet(10,0);

-			motorSet(1,0);

 

-		}

-		/* lRot=encoderGet(encoder1);

-		rRot=encoderGet(encoder2);

-		lDist=(lRot*pi)/120;

-		rDist=(rRot*pi)/120;

-		lcdPrint(uart2,1,"iHeading: %d",iHeading);

-		if(digitalRead(1)==0){

-		motorSet(1,120);

-		}

-		else if(digitalRead(2)==0){

-		motorSet(10,-120);

-		}

-		else{

-		motorSet(1,0);

-		motorSet(10,0);

-		}

-		*/

+		//get time 1 the first time

+		heading=((lDist-rDist)/(track*2*PI))*360+startTheta;//heading in degrees. might need to be radians for cosine functions

+		//get time 2

+		deltaPos=(lVel+rVel)/2*deltaTime;

+		xPos+=cos(heading)*deltaPos;

+		yPos+=sin(heading)*deltaPos;

+		//get time 1

 		delay(20);

 

 	}

diff --git a/bin/auto.o b/bin/auto.o
new file mode 100644
index 0000000..d06291a
--- /dev/null
+++ b/bin/auto.o
diff --git a/bin/init.o b/bin/init.o
new file mode 100644
index 0000000..eebfb38
--- /dev/null
+++ b/bin/init.o
diff --git a/bin/opcontrol.o b/bin/opcontrol.o
new file mode 100644
index 0000000..8d29c81
--- /dev/null
+++ b/bin/opcontrol.o
diff --git a/bin/output.bin b/bin/output.bin
new file mode 100755
index 0000000..1eef409
--- /dev/null
+++ b/bin/output.bin
diff --git a/bin/zephyr.o b/bin/zephyr.o
new file mode 100644
index 0000000..c9daf0a
--- /dev/null
+++ b/bin/zephyr.o
diff --git a/src/auto.c b/src/auto.c
index 2332428..098412e 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,4 +1,5 @@
 #include <main.h>

+#include <zephyr.c>

 

 /*#define TARGET_SPEED 65

 

diff --git a/src/zephyr.c b/src/zephyr.c
index 7bfd76e..109742f 100644
--- 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