diff options
Diffstat (limited to 'src/opcontrol.c')
| -rw-r--r-- | src/opcontrol.c | 62 |
1 files changed, 54 insertions, 8 deletions
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 <main.h>
+#include <math.h>
+
+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);
}
|