From cb70636b207510a3da66573284607e9ae6459e32 Mon Sep 17 00:00:00 2001 From: frydaddy07 Date: Fri, 4 Dec 2015 22:39:49 -0500 Subject: Positioning Attempt This might work. Clyne should adapt and maybe use tomorrow --- .../Shooter Testing/src/opcontrol.c | 66 ++++----------------- bin/auto.o | Bin 0 -> 5352 bytes bin/init.o | Bin 0 -> 1116 bytes bin/opcontrol.o | Bin 0 -> 1988 bytes bin/output.bin | Bin 0 -> 19736 bytes bin/zephyr.o | Bin 0 -> 5232 bytes src/auto.c | 1 + src/zephyr.c | 2 + 8 files changed, 15 insertions(+), 54 deletions(-) create mode 100644 bin/auto.o create mode 100644 bin/init.o create mode 100644 bin/opcontrol.o create mode 100755 bin/output.bin create mode 100644 bin/zephyr.o 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 Binary files /dev/null and b/bin/auto.o differ diff --git a/bin/init.o b/bin/init.o new file mode 100644 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 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 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 index 0000000..c9daf0a Binary files /dev/null and b/bin/zephyr.o differ 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 +#include /*#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 #include +#define PI =3.14159 + #include #ifdef LCD_PORT -- cgit v1.2.3