From 0fb3c40c38905790f47cbd77f7e26553bb0061fa Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Wed, 2 Dec 2015 19:43:40 -0500 Subject: RPM/cannon stuff --- .../Shooter Testing/src/opcontrol.c | 34 +----------- include/zephyr.h | 2 + src/opcontrol.c | 62 +++++++++++++++++++--- src/zephyr.c | 38 ++++++------- 4 files changed, 77 insertions(+), 59 deletions(-) diff --git a/Nate's Position Testing/Shooter Testing/src/opcontrol.c b/Nate's Position Testing/Shooter Testing/src/opcontrol.c index 66fe496..3ac3d9b 100644 --- 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 - * @brief File for operator control code - * - * This file should contain the user operatorControl() function and any functions related to it. - * - * Copyright (c) 2011-2014, Purdue University ACM SIG BOTS. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of Purdue University ACM SIG BOTS nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL PURDUE UNIVERSITY ACM SIG BOTS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Purdue Robotics OS contains FreeRTOS (http://www.freertos.org) whose source code may be - * obtained from http://sourceforge.net/projects/freertos/files/ or on request. - */ #include "main.h" + #define round(x) ((x)>=0?(long)((x)+0.5):(long)((x)-0.5)) /* * Runs the user operator control code. This function will be started in its own task with the diff --git a/include/zephyr.h b/include/zephyr.h index 844416a..1ba234c 100644 --- a/include/zephyr.h +++ b/include/zephyr.h @@ -3,6 +3,8 @@ #include +#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 e5955eb..9eefe95 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -1,10 +1,18 @@ #include +#include + +const char *song = "John Cena:d=4,o=5,b=125:4p,4g5,8a5,8f5,8p,1g5,4p,4a#5,8a5,8f5,8p,1g5"; +const char *xpst = "WinXP Login:d=4,o=5,b=160:4d#6.,4a#5.,2g#5.,4d#6,4a#5"; +const char *xpst2 = "WinXP Shutdown:d=4,o=5,b=125:4g#6,4d#6,4g#5,2a#5"; +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,\ +4a#5,8g5,8f5,4d#5"; +static unsigned char cyc = 0; void lcdUpdateFunc(void *); void operatorControl(){ - static char lift; + static char lift,cann; zLCDStart(); zLCDSetUpdateFunc(lcdUpdateFunc); @@ -19,27 +27,65 @@ void operatorControl(){ // Set the rotating motor speed. - zMotorSet("ROTATER",-zJoyAnalog(2,1)/4); + zMotorSet("Rotater",-zJoyAnalog(2,1)/4); // Set the intake's speed. - zMotorSet("INTAKE",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127)); + zMotorSet("Intake",zGetDigitalMotorSpeed(1,6,JOY_UP,JOY_DOWN,127)); // Set the lift's speed. lift=zGetDigitalMotorSpeed(2,6,JOY_UP,JOY_DOWN,127); - zMotorSet("LIFT1",lift); - zMotorSet("LIFT2",lift); + zMotorSet("Lift 1",lift); + zMotorSet("Lift 2",lift); + + cann=zJoyAnalog(2,3);//zGetDigitalMotorSpeed(1,5,JOY_UP,JOY_DOWN,127); + + zMotorSet("Left cannon" ,-cann); + zMotorSet("Right cannon", cann); + + zMotorSet("Misc",zGetDigitalMotorSpeed(2,7,JOY_UP,JOY_DOWN,127)); + + if(zJoyDigital(1,7,JOY_LEFT)){ + speakerInit(); + switch(cyc){ + case 0:speakerPlayRtttl(song );break; + case 1:speakerPlayRtttl(xpst );break; + case 2:speakerPlayRtttl(xpst2);break; + case 3:speakerPlayRtttl(bound);break; + } + if(++cyc == 4) cyc = 0; + speakerShutdown(); + } delay(10); // Short delay to allow task switching } } void lcdUpdateFunc(void *unused_param){ - static double liftIME; + static double l,r,dist,heading; + static double cl,cr,ca; + + /* + * Positioning code. + */ + + l = zMotorIMEGet("Left drive") / 627.2L; + r = -zMotorIMEGet("Right drive") / 627.2L; + + dist=(l - r) * 8.64L; + heading = fmod(round(dist / 15),360.0L); + + zLCDWrite(1,"%.3lf %.3lf",heading,dist); + //zLCDWrite(2,"%.3lf %.3lf",l,r); - liftIME = (zMotorIMEGetVelocity("LIFT1") - zMotorIMEGetVelocity("LIFT2")) / 2 / 16.3333125L; + /* + * RPM control code. + */ - zLCDWrite(1,"%.3lf",liftIME); + cl = -zMotorIMEGetVelocity("Left cannon") / 16.3333125L * 9; + cr = zMotorIMEGetVelocity("Right cannon") / 16.3333125L * 9; + ca = (cl + cr) / 2; + zLCDWrite(2,"RPM: %.3lf",ca); } diff --git a/src/zephyr.c b/src/zephyr.c index 847f95a..7bfd76e 100644 --- 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 -- cgit v1.2.3