static double rpm = 0,\r
trpm = 1850,\r
arpm = 0;\r
+static bool cannReady = false;\r
\r
/**\r
* Contains the current X and Y position in inches. The X axis extends from\r
* inches per millisecond\r
*/\r
\r
- lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 8.64L / 60000;\r
- rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 8.64L / 60000;\r
+ lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 12.566L / 60000;\r
+ rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 12.566L / 60000;\r
\r
/**\r
* Get the distance thing.\r
*/\r
\r
cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5);\r
- rangle = zGyroGet() - (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);\r
+ rangle = zGyroGet() + (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);\r
\r
lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle);\r
\r
\r
void cannonProc(void *procPtr){\r
static double cl,cr,ca;\r
- static int speed;\r
+ static int speed;//,ispeed = 0;\r
\r
cannonProcRun = true;\r
\r
speed += 2;\r
zMotorSet("Left cannon" ,-speed,2);\r
zMotorSet("Right cannon", speed,2);\r
+ cannReady = false;\r
}else if(ca > trpm + 40){\r
speed -= 2;\r
+ //if(speed < ispeed) speed = ispeed;\r
zMotorSet("Left cannon" ,-speed,2);\r
zMotorSet("Right cannon", speed,2);\r
+ cannReady = false;\r
+ }else{\r
+ cannReady = true;\r
+ //ispeed = speed;\r
}\r
\r
lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm);\r
delay(100);\r
}\r
\r
- zMotorSet("Left cannon" ,0,2);\r
zMotorSet("Right cannon",0,2);\r
\r
zMotorReturn("Left cannon");\r
*/\r
\r
void lcdUpdateFunc(void *unused_param){\r
- unsigned long elapsed;\r
while(1){\r
/*\r
* Track elapsed time since operatorControl() entrance.\r
delay(LCD_RATE);\r
}\r
}\r
+\r
+void autonomous(){\r
+ static unsigned long elapsed = 0;\r
+ opmillis = millis();\r
+ taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);\r
+ while(1){\r
+ elapsed = millis() - opmillis;\r
+ lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));\r
+\r
+ if(cannReady){\r
+ zMotorSet("Misc",127,0);\r
+ delay(500);\r
+ zMotorSet("Misc",-127,0);\r
+ delay(500);\r
+ zMotorSet("Misc",0,0);\r
+ }\r
+ delay(10);\r
+ }\r
+}\r