diff options
| author | Clyne Sullivan <tullivan99@gmail.com> | 2016-01-13 16:03:09 -0500 |
|---|---|---|
| committer | Clyne Sullivan <tullivan99@gmail.com> | 2016-01-13 16:03:09 -0500 |
| commit | 476a0dbf41e14458c52a5cd2fe5b4c050a6c6551 (patch) | |
| tree | de9499df81b4db0e124c8372279274b34357cae7 | |
| parent | 3ef0172201638e9d35ef7920add2cdde046ccbc1 (diff) | |
prog skills
| -rw-r--r-- | src/auto.c | 10 | ||||
| -rw-r--r-- | src/opcontrol.c | 36 |
2 files changed, 35 insertions, 11 deletions
@@ -1,10 +1,10 @@ #include <main.h>
#include <zephyr.h>
-#define TARGET_RPM 1700
+//#define TARGET_RPM 1700
-void autonomous(){
- /*static double cl,cr,ca;
+/*void autonomous(){
+ static double cl,cr,ca;
static char speed;
speed = 30;
@@ -25,5 +25,5 @@ void autonomous(){ delay(900);
zMotorSet("Misc",0);
- motorStopAll();*/
-}
+ motorStopAll();
+}*/
diff --git a/src/opcontrol.c b/src/opcontrol.c index 1d9a0a7..41da208 100644 --- a/src/opcontrol.c +++ b/src/opcontrol.c @@ -50,6 +50,7 @@ static char cann = 0; static double rpm = 0,
trpm = 1850,
arpm = 0;
+static bool cannReady = false;
/**
* Contains the current X and Y position in inches. The X axis extends from
@@ -321,8 +322,8 @@ void moveProc(void *unused_param){ * inches per millisecond
*/
- lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 8.64L / 60000;
- rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 8.64L / 60000;
+ lv = zMotorIMEGetVelocity("Left drive") / 39.2L * 12.566L / 60000;
+ rv = -zMotorIMEGetVelocity("Right drive") / 39.2L * 12.566L / 60000;
/**
* Get the distance thing.
@@ -388,7 +389,7 @@ void aimProc(void *procPtr){ */
cangle = (int)floor(zMotorIMEGet("Rotater") / 627.2L * 112.5);
- rangle = zGyroGet() - (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);
+ rangle = zGyroGet() + (atan(ypos / (GOAL_DISTANCE - xpos)) * 180 / PI);
lcdPrint(uart1,1,"%.3lf, %.3lf",cangle,rangle);
@@ -422,7 +423,7 @@ void aimProc(void *procPtr){ void cannonProc(void *procPtr){
static double cl,cr,ca;
- static int speed;
+ static int speed;//,ispeed = 0;
cannonProcRun = true;
@@ -524,10 +525,16 @@ void cannonProc(void *procPtr){ speed += 2;
zMotorSet("Left cannon" ,-speed,2);
zMotorSet("Right cannon", speed,2);
+ cannReady = false;
}else if(ca > trpm + 40){
speed -= 2;
+ //if(speed < ispeed) speed = ispeed;
zMotorSet("Left cannon" ,-speed,2);
zMotorSet("Right cannon", speed,2);
+ cannReady = false;
+ }else{
+ cannReady = true;
+ //ispeed = speed;
}
lcdPrint(uart1,2,"%.0lf|%.3lf\n",trpm,rpm);
@@ -536,7 +543,6 @@ void cannonProc(void *procPtr){ delay(100);
}
- zMotorSet("Left cannon" ,0,2);
zMotorSet("Right cannon",0,2);
zMotorReturn("Left cannon");
@@ -633,7 +639,6 @@ PUSH: */
void lcdUpdateFunc(void *unused_param){
- unsigned long elapsed;
while(1){
/*
* Track elapsed time since operatorControl() entrance.
@@ -647,3 +652,22 @@ void lcdUpdateFunc(void *unused_param){ delay(LCD_RATE);
}
}
+
+void autonomous(){
+ static unsigned long elapsed = 0;
+ opmillis = millis();
+ taskCannon = taskCreate(cannonProc,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
+ while(1){
+ elapsed = millis() - opmillis;
+ lcdPrint(uart1,1,"%02d:%02d",(int)(elapsed / 60000),(int)((elapsed / 1000) % 60));
+
+ if(cannReady){
+ zMotorSet("Misc",127,0);
+ delay(500);
+ zMotorSet("Misc",-127,0);
+ delay(500);
+ zMotorSet("Misc",0,0);
+ }
+ delay(10);
+ }
+}
|