From cebb17f85536815daf55fd8196791ee719ba6ba0 Mon Sep 17 00:00:00 2001
From: Clyne Sullivan <tullivan99@gmail.com>
Date: Fri, 13 Nov 2015 07:05:34 -0500
Subject: everything
---
include/main.h | 28 +++++++
src/auto.c | 42 +++++++++++
src/init.c | 31 +++++++-
src/opcontrol.c | 225 ++++++++++++++++++++++++++++++++++++++++----------------
4 files changed, 261 insertions(+), 65 deletions(-)
diff --git a/include/main.h b/include/main.h
index ebe72a6..6d36973 100644
--- a/include/main.h
+++ b/include/main.h
@@ -11,6 +11,34 @@
extern "C" {
#endif
+/*
+ * Aliases for all the motors, stored in an enum for convenience.
+*/
+
+enum MOTOR_PORT_MAP {
+ UNUSED = 0,
+ CANNON1,
+ CANNON2,
+ CANNON3,
+ CANNON4,
+ INTAKE,
+ DRIVER,
+ DRIVEL,
+ LIFT1,
+ LIFT2,
+ ROTATER,
+};
+
+enum IME_PORT_MAP {
+ IDRIVER,
+ IDRIVEL,
+ IROTATER,
+ ILIFT1,
+ ILIFT2,
+ ICANNON3
+};
+
+
void autonomous();
void initializeIO();
void initialize();
diff --git a/src/auto.c b/src/auto.c
index 50fda48..33204f9 100644
--- a/src/auto.c
+++ b/src/auto.c
@@ -1,4 +1,46 @@
#include <main.h>
+#define TARGET_RPM 1970
+
+static unsigned int time;
+
+void autoDelay(unsigned int ms){
+ delay(ms);
+ time-=ms;
+}
+
void autonomous(){
+ static int trpm;
+ static double rpm;
+ static char cs;
+
+ time = 15000;
+ cs = rpm = trpm = 0;
+
+ do{
+
+ cs+=cs<100?10:0;
+
+ motorSet(CANNON1,cs);
+ motorSet(CANNON2,cs);
+ motorSet(CANNON3,cs);
+ motorSet(CANNON4,cs);
+
+ autoDelay(800);
+
+ imeGetVelocity(ICANNON3,&trpm);
+ rpm=abs(trpm)/24.5L;
+ rpm*=25.0L;
+
+ }while(rpm<TARGET_RPM);
+
+ motorSet(LIFT1,127);
+ motorSet(LIFT2,127);
+
+ delay(time);
+
+ // Stop 4000ms remaining
+
+ motorStopAll();
+
}
diff --git a/src/init.c b/src/init.c
index e2145b4..3ffa429 100644
--- a/src/init.c
+++ b/src/init.c
@@ -4,13 +4,38 @@ Gyro gyro;
unsigned int imeCount;
void initializeIO(){
- pinMode(ANALOG_PORT(1),INPUT_ANALOG);
+ pinMode(ANALOG_PORT(1),INPUT_ANALOG); // Power expander status port
+ pinMode(1,OUTPUT); // LED
}
void initialize(){
- imeCount = imeInitializeAll();
- gyro=gyroInit(2,0);
+
+ // Initialize the LCDs.
+
lcdInit(uart1);
lcdClear(uart1);
lcdSetBacklight(uart1,1);
+
+ lcdInit(uart2);
+ lcdClear(uart2);
+ lcdSetBacklight(uart2,1);
+
+ // Setup sensors.
+
+ imeCount = imeInitializeAll();
+ gyro=gyroInit(2,0);
+
+ lcdPrint(uart1,1,"%u IMEs :)",imeCount);
+
+ lcdPrint(uart2,1,"ERR: Dark theme ");
+ lcdPrint(uart2,2," not found! ");
+
+ delay(2000);
+
+ lcdPrint(uart1,1,"ready...");
+ lcdPrint(uart2,1,"...yeah. ");
+ lcdPrint(uart2,2," ");
+
+ delay(1000);
+
}
diff --git a/src/opcontrol.c b/src/opcontrol.c
index bd3118a..b138bc0 100644
--- a/src/opcontrol.c
+++ b/src/opcontrol.c
@@ -1,102 +1,203 @@
#include <main.h>
#include <stdint.h>
-#include <string.h>
-
-enum MOTOR_PORT_MAP {
- UNUSED = 0,
- INTAKE1,
- DRIVEL ,
- DRIVER ,
- LIFT1 ,
- LIFT2 ,
- UNUSED6,
- INTAKE2,
- INTAKE3,
- INTAKE4,
- ROTATER,
-};
-
-extern Gyro gyro;
+#include <math.h>
+
+extern Gyro gyro; // Originally defined in init.c
+
+/*
+ * These keep track of the current LCD 'window' and the number of 'windows' that exist.
+*/
static signed int ctty=0;
-const unsigned int mtty=3;
+const unsigned int mtty=4;
+
+static double imeDriveLeft;
+static double imeDriveRight;
+static double imeRotater;
+
+static double gyroRotation;
+
+/*
+ * This runs as a separate task to update the LCD.
+*/
void operatorLCD(void *unused){
+ static int ime;
+
while(1){
+
+ /*
+ * Display information according to the current 'window'.
+ */
+
switch(ctty){
default:
- case 0:
+
+ case 0: // Welcome screen
+
lcdSetText(uart1,1," JOHN CENA ");
- lcdSetText(uart1,2," ============ ");
+ lcdPrint(uart1,2," ===========%3d",motorGet(CANNON1));
break;
- case 1:
+
+ case 1: // Battery levels in volts
+
lcdPrint(uart1,1,"MAIN: %0.3f",(float)(powerLevelMain()/1000.0f));
- lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1) / 210.0f));
+ lcdPrint(uart1,2,"EXP : %0.3f",(float)(analogRead(1) / 280.0f));
+ break;
+
+ case 2: // Gyroscope readings
+
+ imeGet(IROTATER,&ime);
+
+ double rot = ime / 3.0L / 627.2L * 10;
+
+ rot = floor(rot) / 10;
+
+ lcdPrint(uart1,1,"Gyro: %d",gyroGet(gyro));
+ lcdPrint(uart1,2,"Rot.: %.1lf",rot);
+
+ break;
+
+ case 3:
+
+ lcdPrint(uart1,1,"%.3lf",imeDriveLeft);
+ lcdPrint(uart1,2,"%.3lf",imeDriveRight);
+
break;
- case 2:
- lcdPrint(uart1,1,"Gyro: %u",gyroGet(gyro));
}
- delay(2000);
+
+ delay(500);
+ }
+}
+
+void operatorPoll(){
+ static int idl,idr,ir,gr;
+ while(1){
+
+ /*
+ * Read in rotations of motors and the current angle of the robot.
+ */
+
+ imeGet(DRIVEL,&idl); // Get reading
+ imeDriveLeft = idl / 627.0L; // Convert to wheel rotations
+ imeDriveLeft *= 8.64L; // Convert to distance in inches
+
+ imeGet(DRIVER,&idr); // Get reading
+ imeDriveRight = idr / 627.0L; // Convert to wheel rotations
+ imeDriveRight *= 8.64L; // Convert to distance in inches
+
+ imeGet(ROTATER,&ir);
+ imeRotater = ir / 627.0L;
+
+ gyroRotation = gyroGet(gyro);
+
+ delay(100);
}
}
void operatorControl(){
-#define getJoy(n) joy[n-1]
+ static bool selfAim = false;
- static int8_t joy[8],
- lift = 0,
- rotate = 0,
- intake = 0,
- uiinc = 0;
+ static char uiinc = 0; // See below
- // Start the LCD task
- taskCreate(operatorLCD,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
+ static char in,lift; // Used to set the multiple cannon motors to the same joy-stick reading.
+
+ taskCreate(operatorLCD ,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
+ taskCreate(operatorPoll,TASK_DEFAULT_STACK_SIZE,NULL,TASK_PRIORITY_DEFAULT);
while(1){
- joy[0]=joystickGetAnalog(1,1);
- joy[1]=joystickGetAnalog(1,2);
- joy[2]=joystickGetAnalog(1,3);
- joy[3]=joystickGetAnalog(1,4);
- joy[4]=joystickGetAnalog(2,1);
- joy[5]=joystickGetAnalog(2,2);
- joy[6]=joystickGetAnalog(2,3);
- joy[7]=joystickGetAnalog(2,4);
-
- intake = getJoy(7);
- rotate =-getJoy(5);
- lift = joystickGetDigital(2,6,JOY_UP ) ? 127 :
- joystickGetDigital(2,6,JOY_DOWN) ? -127 :
- 0;
-
- motorSet(INTAKE1,intake);
- motorSet(INTAKE2,intake);
- motorSet(INTAKE3,intake);
- motorSet(INTAKE4,intake);
- motorSet(ROTATER,rotate);
+ // Set the drive motors speeds.
+
+ motorSet(DRIVEL,joystickGetAnalog(1,3));
+ motorSet(DRIVER,joystickGetAnalog(1,2));
+
+ // Set the rotating motor speed.
+
+ if(!selfAim){
+
+ motorSet(ROTATER,-joystickGetAnalog(2,1)/2);
+
+ }else{
+
+ //static int gc=0,gl=0;
+
+ //gl = gc;
+ //gc = gyroGet(gyro);
+
+ }
+
+ // Set the cannon's speed.
+
+ in=joystickGetAnalog(2,3);
+
+ motorSet(CANNON1,in);
+ motorSet(CANNON2,in);
+ motorSet(CANNON3,in);
+ motorSet(CANNON4,in);
+
+ // Set the intake's speed.
+
+ motorSet(INTAKE,joystickGetDigital(1,6,JOY_UP )? 127 :
+ joystickGetDigital(1,6,JOY_DOWN)? -127 :
+ 0 );
+
+ // Set the lift's speed.
+
+ lift=joystickGetDigital(2,6,JOY_UP )? 127 :
+ joystickGetDigital(2,6,JOY_DOWN)? -127 :
+ 0 ;
+
motorSet(LIFT1,lift);
motorSet(LIFT2,lift);
- motorSet(DRIVEL,-getJoy(2));
- motorSet(DRIVER, getJoy(3));
-
- /*motorSet(PULLER,joystickGetDigital(2,6,JOY_UP ) ? 127 :
- joystickGetDigital(2,6,JOY_DOWN) ? -127 :
- 0);*/
+ /*
+ * Miscellaneous operation handlers.
+ */
- if(++uiinc==20){
+ if(++uiinc==20){ // Equates to every 200ms
uiinc=0;
+
+ // Goto next 'window'.
+
if(joystickGetDigital(1,7,JOY_UP) ||
joystickGetDigital(2,7,JOY_UP) ){
if(++ctty==mtty)ctty=0;
}
- else if(joystickGetDigital(1,7,JOY_DOWN) ||
+
+ // Goto previous 'window'.
+
+ if(joystickGetDigital(1,7,JOY_DOWN) ||
joystickGetDigital(2,7,JOY_DOWN) ){
if(--ctty==-1)ctty=mtty-1;
}
+
+ // Run autonomous (for testing wo/ competition switch).
+
+ if(joystickGetDigital(1,7,JOY_RIGHT))
+ autonomous();
+
+ // Goto test area.
+
+ if(joystickGetDigital(1,7,JOY_LEFT)){
+ static double target = 0;
+
+ if(!target) target = (imeDriveLeft+imeDriveRight) / 2 + 13;
+
+ motorSet(DRIVEL,60);
+ motorSet(DRIVER,60);
+
+ while((imeDriveLeft+imeDriveRight) / 2 < target)
+ delay(10);
+
+ motorSet(DRIVEL,0);
+ motorSet(DRIVER,0);
+
+ }
+
}
- delay(10);
+ delay(10); // Short delay to allow task switching.
}
}
--
cgit v1.2.3