RPM/cannon stuff

diff --git a/Nate's Position Testing/Shooter Testing/src/opcontrol.c b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
index 66fe496589a9810941366ea1e75eb449d73b28c7..3ac3d9bf7c0f1ea0600753cc2ac2b478728a648a 100644 (file)
--- a/Nate's Position Testing/Shooter Testing/src/opcontrol.c
+++ b/Nate's Position Testing/Shooter Testing/src/opcontrol.c
@@ -1,37 +1,5 @@
-/** @file opcontrol.c\r
- * @brief File for operator control code\r
- *\r
- * This file should contain the user operatorControl() function and any functions related to it.\r
- *\r
- * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS.\r
- * All rights reserved.\r
- *\r
- * Redistribution and use in source and binary forms, with or without\r
- * modification, are permitted provided that the following conditions are met:\r
- *     * Redistributions of source code must retain the above copyright\r
- *       notice, this list of conditions and the following disclaimer.\r
- *     * Redistributions in binary form must reproduce the above copyright\r
- *       notice, this list of conditions and the following disclaimer in the\r
- *       documentation and/or other materials provided with the distribution.\r
- *     * Neither the name of Purdue University ACM SIG BOTS nor the\r
- *       names of its contributors may be used to endorse or promote products\r
- *       derived from this software without specific prior written permission.\r
- *\r
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND\r
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED\r
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE\r
- * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY\r
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES\r
- * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\r
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND\r
- * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT\r
- * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS\r
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
- *\r
- * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be\r
- * obtained from http://sourceforge.net/projects/freertos/files/ or on request.\r
- */\r
 #include "main.h"\r
+\r
 #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))\r
 /*\r
  * Runs the user operator control code. This function will be started in its own task with the\r

diff --git a/include/zephyr.h b/include/zephyr.h
index 844416aace7b2df80455df4d90d47a1be7cb2e29..1ba234c801dd58c90942764dc5f805274dfead38 100644 (file)
--- a/include/zephyr.h
+++ b/include/zephyr.h
@@ -3,6 +3,8 @@
 
 #include <main.h>
 
+#define PI     3.14159265L
+
 #define APPLY_THRESH(n,t)      if(n < t && n > -t){ n = 0;}
 
 /*

diff --git a/src/opcontrol.c b/src/opcontrol.c
index e5955ebfe2b1aa1c1513247dabc28c713154b23b..9eefe951195b61f283ecb2894736ac5266104064 100644 (file)
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,10 +1,18 @@
 #include <main.h>\r
+#include <math.h>\r
+\r
+const char *song  = "John Cena:d=4,o=5,b=125:4p,4g5,8a5,8f5,8p,1g5,4p,4a#5,8a5,8f5,8p,1g5";\r
+const char *xpst  = "WinXP Login:d=4,o=5,b=160:4d#6.,4a#5.,2g#5.,4d#6,4a#5";\r
+const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5";\r
+const char *bound = "Nobody to Love:d=4,o=5,b=125:4d#5,2g5.,4g5,4f5,2g5,4d#5,4f5,2g5,4d#5,4f5,4g5,4d#5,4d#5,4f5,4g5,4a#4,1c5,4d#5,4f5,2g5,\\r
+4a#5,8g5,8f5,4d#5";\r
+static unsigned char cyc = 0;\r
 \r
 void lcdUpdateFunc(void *);\r
 \r
 void operatorControl(){\r
 \r
-       static char lift;\r
+       static char lift,cann;\r
 \r
        zLCDStart();\r
        zLCDSetUpdateFunc(lcdUpdateFunc);\r
@@ -19,27 +27,65 @@ void operatorControl(){
 \r
                //      Set the rotating motor speed.\r
 \r
-               zMotorSet("ROTATER",-zJoyAnalog(2,1)/4);\r
+               zMotorSet("Rotater",-zJoyAnalog(2,1)/4);\r
 \r
                // Set the intake's speed.\r
 \r
-               zMotorSet("INTAKE",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));\r
+               zMotorSet("Intake",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127));\r
 \r
                // Set the lift's speed.\r
 \r
                lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127);\r
 \r
-               zMotorSet("LIFT1",lift);\r
-               zMotorSet("LIFT2",lift);\r
+               zMotorSet("Lift 1",lift);\r
+               zMotorSet("Lift 2",lift);\r
+\r
+               cann=zJoyAnalog(2,3);//zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127);\r
+\r
+               zMotorSet("Left cannon" ,-cann);\r
+               zMotorSet("Right cannon", cann);\r
+\r
+               zMotorSet("Misc",zGetDigitalMotorSpeed(2,7,JOY_UP,JOY_DOWN,127));\r
+\r
+               if(zJoyDigital(1,7,JOY_LEFT)){\r
+                       speakerInit();\r
+                       switch(cyc){\r
+                       case 0:speakerPlayRtttl(song );break;\r
+                       case 1:speakerPlayRtttl(xpst );break;\r
+                       case 2:speakerPlayRtttl(xpst2);break;\r
+                       case 3:speakerPlayRtttl(bound);break;\r
+                       }\r
+                       if(++cyc == 4) cyc = 0;\r
+                       speakerShutdown();\r
+               }\r
 \r
                delay(10);      // Short delay to allow task switching\r
        }\r
 }\r
 \r
 void lcdUpdateFunc(void *unused_param){\r
-       static double liftIME;\r
+       static double l,r,dist,heading;\r
+       static double cl,cr,ca;\r
+\r
+       /*\r
+        *      Positioning code.\r
+       */\r
+\r
+       l =  zMotorIMEGet("Left drive")  / 627.2L;\r
+       r = -zMotorIMEGet("Right drive") / 627.2L;\r
+\r
+       dist=(l - r) * 8.64L;\r
+       heading = fmod(round(dist / 15),360.0L);\r
+\r
+       zLCDWrite(1,"%.3lf %.3lf",heading,dist);\r
+       //zLCDWrite(2,"%.3lf %.3lf",l,r);\r
 \r
-       liftIME = (zMotorIMEGetVelocity("LIFT1") - zMotorIMEGetVelocity("LIFT2")) / 2 / 16.3333125L;\r
+       /*\r
+        *      RPM control code.\r
+       */\r
 \r
-       zLCDWrite(1,"%.3lf",liftIME);\r
+       cl = -zMotorIMEGetVelocity("Left cannon")  / 16.3333125L * 9;\r
+       cr =  zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9;\r
+       ca = (cl + cr) / 2;\r
+       zLCDWrite(2,"RPM: %.3lf",ca);\r
 }\r

diff --git a/src/zephyr.c b/src/zephyr.c
index 847f95a2b96aed435fb104b646953375b28e546c..7bfd76ed0f3c144dd3245029650d5c1cd7c64560 100644 (file)
--- a/src/zephyr.c
+++ b/src/zephyr.c
@@ -81,30 +81,32 @@ void zGyroInit(void){
 #define MOTOR_PORT_COUNT       10
 
 #ifdef IME_ENABLE
-#define MOTOR_IME_COUNT                5
+#define MOTOR_IME_COUNT                7
 #endif // IME_ENABLE
 
 const char *MOTOR_PORT_MAP[MOTOR_PORT_COUNT] = {
-       "UNUSED1",
-       "UNUSED2",
-       "UNUSED3",
-       "UNUSED4",
-       "INTAKE",
-       "DRIVER",
-       "DRIVEL",
-       "LIFT1",
-       "LIFT2",
-       "ROTATER"
+       "Left cannon",
+       "Right cannon",
+       "Misc",
+       "Port 4",
+       "Intake",
+       "Right drive",
+       "Left drive",
+       "Lift 1",
+       "Lift 2",
+       "Rotater"
 };
 
 #ifdef IME_ENABLE
 
 const char *MOTOR_IME_MAP[MOTOR_IME_COUNT] = {
-       "DRIVER",
-       "DRIVEL",
-       "ROTATER",
-       "LIFT1",
-       "LIFT2"
+       "Right drive",
+       "Left drive",
+       "Rotater",
+       "Lift 1",
+       "Lift 2",
+       "Left cannon",
+       "Right cannon"
 };
 
 static unsigned int imeCount = 0;
@@ -179,8 +181,8 @@ void zDriveUpdate(void){
        APPLY_THRESH(l,DRIVE_THRESHOLD);
        APPLY_THRESH(r,DRIVE_THRESHOLD);
 
-       zMotorSet("DRIVEL",l);
-       zMotorSet("DRIVER",r);
+       zMotorSet("Left drive",l);
+       zMotorSet("Right drive",r);
 
 #endif // DRIVE_NORMAL