Skip to content

Instantly share code, notes, and snippets.

@watsuprico
Last active September 24, 2019 02:04
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save watsuprico/bf51e2ce3be7b9fe525d4a3297319211 to your computer and use it in GitHub Desktop.
Save watsuprico/bf51e2ce3be7b9fe525d4a3297319211 to your computer and use it in GitHub Desktop.
Shrimp Industries: S. S. Shrimp (Base v1.1) - SkillsUSA: Mobile Robotics (2019) Competition Code
#pragma config(Sensor, dgtl1, Green1, sensorLEDtoVCC)
#pragma config(Sensor, dgtl2, Green2, sensorLEDtoVCC)
#pragma config(Sensor, dgtl3, Yellow1, sensorLEDtoVCC)
#pragma config(Sensor, dgtl4, Yellow2, sensorLEDtoVCC)
#pragma config(Sensor, dgtl5, Red1, sensorLEDtoVCC)
#pragma config(Sensor, dgtl6, Red2, sensorLEDtoVCC)
#pragma config(Sensor, dgtl12, DEBUG, sensorTouch)
#pragma config(Motor, port2, RightTrackMotor, tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor, port3, BottomLiftMotor, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port4, TopLiftMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, ClawRotationMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, LiftRotationMotor, tmotorServoStandard, openLoop)
#pragma config(Motor, port7, RearFlipperMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, ClawMotor, tmotorVex269_MC29, openLoop, reversed)
#pragma config(Motor, port9, LeftTrackMotor, tmotorVex393_MC29, openLoop, driveLeft)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"
/*
__ _ ___
(_ |_ __ o __ |_) | __ _| _ _|_ __ o _ _
__)| | | | |||| _|_| |(_||_|_> |_ | | (/__>
presents
the
________ ________ ________ ___ ___ ________ ___ _____ ______ ________
|\ ____\ |\ ____\ |\ ____\ |\ \|\ \ |\ __ \ |\ \ |\ _ \ _ \ |\ __ \
\ \ \___|_ \ \ \___|_ \ \ \___|_ \ \ \\\ \ \ \ \|\ \ \ \ \ \ \ \\\__\ \ \ \ \ \|\ \
\ \_____ \ \ \_____ \ \ \_____ \ \ \ __ \ \ \ _ _\ \ \ \ \ \ \\|__| \ \ \ \ ____\
\|____|\ \ ___ \|____|\ \ \|____|\ \ \ \ \ \ \ \ \ \\ \| \ \ \ \ \ \ \ \ \ \ \ \___|
____\_\ \ |\__\ ____\_\ \ ____\_\ \ \ \__\ \__\ \ \__\\ _\ \ \__\ \ \__\ \ \__\ \ \__\
|\_________\\|__| |\_________\ |\_________\ \|__|\|__| \|__|\|__| \|__| \|__| \|__| \|__|
\|_________| \|_________| \|_________|
BASE V1.1
** Designed for Turning Point (2019) **
__________________________________________________________________________
/ REMOTE CONTROL BUTTON MAPPING \
| [ Btn5D ] - Linear Lift DOWN - [ Btn6U ] |
| [ Btn5U ] - Linear Lift UP - [ Btn6U ] |
| ------------------------------------------------------------------------ |
| Lift rotation v (go up) | Scoope rotation v (go up) |
| (7U) | (8U) |
| Rear off >(7L) (7R)< Rear On | Claw close >(78) (8R)< Claw Open |
| (7D) | (8D) |
| Lift rotation ^ (go down) | Scoope rotation ^ (go down) |
| | |
| |Left Track| |Right Track| |
| | ____ | | ____ / |
| Left forwards |--/ \ | | / \--| Right forwards |
| Left backwards |--\____/ | | \____/--| Right backwards |
| |
\__________________________________________________________________________/
** Base code file **
|--> User control mode
|--> Autonomous mode (?? probably just full speed motors for x seconds ?? (it works ever year))
|--> Cycle LEDs (int Delay, bool LEDsOn): CycleLEDs(100, false) turns off the LEDs with a sleep(100) between each LED. (bool LEDsOn) is defaulted to "true".
|--> Flash LED(int LED, int Delay): FlashLED(Green1, 100) turns Green1 on, wait 100milliseconds, turn Green1 off, wait 100milliseconds
|--> SwitchDebugMode(): just flashes the LEDs telling the user to release the button
|--> JoystickControls(): Task that allows remote control
\--> TimeKeeper(): task that ticks every 100 milliseconds and then adds 100 to "Time". (Keeps track of run time).
** User control mode **
|--> Clear LCD and display "Shrimp Industries" and "S.S. Shrimp"
|--> Check for digital_port12 to equal 1
|--> If 1, start debug mode. (See debug mode)
|--> Start JoystickControls task
|--> Start TimeKeeper
\--> Flash LEDs if we're good on time, flash red when there's 30 seconds or less, disable LEDs when 10 seconds are left.
*** VARS ***
Time: the variable "TimeKeeper" stores the time in.
UpsideDown: *NOT USED* for servo logic, (decided which way to flip)
DebugEndCounter: the "delay" used for "CycleLEDs" when the debug menu ends or the competition time ends
LCD0: the string displayed in LCD line 0 while debugging (only used in battery tests)
LCD1: the string displayed in LCD line 1 while debugging (only used in battery tests)
LeftLowerDeadZone/RightLowerDeadZone: the "lower" (backwards) deadzone of the remote controller. (must be >0)
LeftUpperDeadZone/RightUpperDeadZone: the "upper" (forwards) deadzone of the remote controller. (must be >0)
****** TIME KEEPER *******
Time keep adds 1 to "Time" every 100 milliseconds. It'll allow time to be tracked for each part.
105000 milliseconds = WHOLE comp. time
ALERT at 75000 millisecs (1:15 (or 30 seconds left))
BLACK OUT at 95000 milliseconds (1:35 (or 10 seconds left))
****** DEBUG MODE *******
To enter "debug mode":
1) Connect a button, touch, or any other on/off switch to digital port 12
2) Press and hold the on/off switch
3) Turn on the robot
4) Whenever the LEDs all turn on and begin to flash, release the button
To exit "debug mode":
You can either go through all the menus, or just restart the bot
If you cycle through all the menus, and are at the point the LEDs are cylcing, plug a on/off switch into dgtl11 and press it. This will exit debug mode.
Navigating debug mode:
1) Once in debug mode, press the button to cycle tests
Tests:
1) Main battery test (current):
[GREEN] - 6.9v (flashing = above 7.1v)
[GREEN] - 6.7v --- Recommended replace line ---
[YELLOW] - 6.4v
[YELLOW] - 6.1v --- REPLACE LINE ---
[RED] - 5.8v
[RED] - 5.6v (flashing = under 5.6v)
[Both red] - VERY low battery level. REPLACE!
2) Main battery test (average): Recommendations are lower due to power draw.
[GREEN] - 6.9v (flashing = above 7.1v)
[GREEN] - 6.7v
[YELLOW] - 6.4v --- Recommended replace line ---
[YELLOW] - 6.1v
[RED] - 5.8v --- REPLACE LINE ---
[RED] - 5.6v (flashing = under 5.6v)
[Both red] - VERY low battery level. REPLACE!
3) Main battery load test: Joystick enabled while current battery level is displayed
[GREEN] - 6.9v (flashing = above 7.1v)
[GREEN] - 6.7v
[YELLOW] - 6.4v --- Recommended replace line ---
[YELLOW] - 6.1v
[RED] - 5.8v --- REPLACE LINE ---
[RED] - 5.6v (flashing = under 5.6v)
[Both red] - VERY low battery level. REPLACE!
4) 9v backup battery test:
[GREEN] - 9v (flashing = above 9.2v)
[GREEN] - 8.8v
[YELLOW] - 8.6v
[YELLOW] - 8.4v
[RED] - 8.2v
[RED] - 8v (flashing = under 7.8v)
5) Enable joystick controls
In this mode, Green1 with flash and Green2 will be solid.
Normal joystick operations are enabled at this time
6) End of debug
LEDs just cycle through. See "How to exit 'debug mode'"
*/
//bool UpsideDown = false;
bool KeepClosed = false;
int DebugEndCounter = 50;
string LCD0 = "";
string LCD1 = "";
int Time = 0;
// The "dead zone" of the controller. (Must be positive)
int LeftLowerDeadZone = 15;
int LeftUpperDeadZone = 15;
int RightLowerDeadZone = 15;
int RightUpperDeadZone = 15;
void CycleLEDs(int TimeDelay, bool On = true)
{
if (On)
{
turnLEDOn(Green1);
sleep(TimeDelay);
turnLEDOn(Green2);
sleep(TimeDelay);
turnLEDOn(Yellow1);
sleep(TimeDelay);
turnLEDOn(Yellow2);
sleep(TimeDelay);
turnLEDOn(Red1);
sleep(TimeDelay);
turnLEDOn(Red2);
sleep(TimeDelay);
}
else
{
turnLEDOff(Green1);
sleep(TimeDelay);
turnLEDOff(Green2);
sleep(TimeDelay);
turnLEDOff(Yellow1);
sleep(TimeDelay);
turnLEDOff(Yellow2);
sleep(TimeDelay);
turnLEDOff(Red1);
sleep(TimeDelay);
turnLEDOff(Red2);
sleep(TimeDelay);
}
}
void FlashLED(int LED, int TimeDelay)
{
turnLEDOn(LED);
sleep(TimeDelay);
turnLEDOff(LED);
sleep(TimeDelay);
}
void SwitchingDebugMenu()
{
clearLCDLine(0);
clearLCDLine(1);
while(SensorValue[DEBUG] == 1)
{
displayLCDString(0,0,"RELEASE");
CycleLEDs(0, false);
sleep(250);
CycleLEDs(0);
clearLCDLine(0);
sleep(250);
}
CycleLEDs(0, false);
clearLCDLine(0);
clearLCDLine(1);
}
void StopAllMotors()
{
motor[port1] = 0;
motor[port2] = 0;
motor[port3] = 0;
motor[port4] = 0;
motor[port5] = 0;
motor[port6] = 0;
motor[port7] = 0;
motor[port8] = 0;
#if defined(VEX2)
motor[port9] = 0;
motor[port10] = 0;
#endif
}
void pre_auton()
{
bStopTasksBetweenModes = true;
}
task autonomous()
{
FlashLED(Yellow1, 100);
FlashLED(Yellow2, 200);
}
task JoyStickControls()
{
while (1)
{
// Rear flipper
if (vexRT[Btn7L])
motor[RearFlipperMotor] = 127;
if (vexRT[Btn7R])
motor[RearFlipperMotor] = 0;
// Movement
if (vexRT[Ch3] > (LeftUpperDeadZone) || vexRT[Ch3] < (LeftLowerDeadZone * -1))
{ motor[LeftTrackMotor] = vexRT[Ch3]/1.5;
motor[port8] = (vexRT[Ch3] / 1.5) * -1;
} else {
motor[LeftTrackMotor] = 0;
motor[port8] = 0;
}
if (vexRT[Ch2] > (RightUpperDeadZone) || vexRT[Ch2] < (RightLowerDeadZone * -1))
{
motor[RightTrackMotor] = vexRT[Ch2]/1.5;
motor[port6] = (vexRT[Ch2] / 1.5) *-1;
} else {
motor[RightTrackMotor] = 0;
motor[port6] = 0;
}
// The linear lift remote functions go here.
if (vexRT[Btn5U] || vexRT[Btn6U]) // We want to go up
{
motor[TopLiftMotor] = 127;
motor[BottomLiftMotor] = 127;
}
else if (vexRT[Btn5D] || vexRT[Btn6D]) // Now down
{
motor[TopLiftMotor] = -127;
motor[BottomLiftMotor] = -127;
}
else
{
motor[TopLiftMotor] = 15;
motor[BottomLiftMotor] = 15;
}
// Rotation (flipping the game piece)
// Lift rotation
if (vexRT[Btn7U])
motor[LiftRotationMotor] = 127;
else if (vexRT[Btn7D])
motor[LiftRotationMotor] = -127;
else
motor[LiftRotationMotor] = 0;
// "Scoope"
/*if (vexRT[Btn8D])
{
// Flip!
if (UpsideDown)
setServo(ScoopeServo, 120);
else
setServo(ScoopeServo, -120);
UpsideDown = !UpsideDown; // Toggle position variable
sleep(50);
}*/
if (vexRT[Btn8D])
motor[ClawRotationMotor] = 40;
else if (vexRT[Btn8U])
motor[ClawRotationMotor] = -40;
else
motor[ClawRotationMotor] = 0;
// Claw
if (vexRT[Btn8L])
{
motor[ClawMotor] = 127;
KeepClosed = true;
}
else if (vexRT[Btn8R])
{
motor[ClawMotor] = -127;
KeepClosed = false;
}
else
{
motor[ClawMotor] = 0;
if (KeepClosed)
motor[ClawMotor] = 15;
else
motor[ClawMotor] = 0;
}
}
}
task TimeKeeper()
{
while (true)
{
Time = Time + 100;
sleep(100);
}
}
//Main
task usercontrol()
{
displayLCDCenteredString(0,"-** Shrimp Industries **-");
displayLCDCenteredString(1,".. S. S. SHRIMP ..");
if (SensorValue[DEBUG] == 1)
{
// Debug mode
CycleLEDs(0, false);
CycleLEDs(250);
sleep(100);
SwitchingDebugMenu();
// We're now offically in debug mode.
sleep(1000);
// Test 1; main battery
clearLCDLine(0);
displayLCDCenteredString(0,"Cur M_Bat test");
while(SensorValue[DEBUG] != 1)
{
clearLCDLine(1);
displayLCDString(1, 0, "Immediate: ");
sprintf(LCD0, "%1.2f%c", (nImmediateBatteryLevel/1000),'V');
displayNextLCDString(LCD0);
if ((nImmediateBatteryLevel/1000) >= 7.1)
{
CycleLEDs(0, false);
FlashLED(Green1, 100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.9 && (nImmediateBatteryLevel/1000) < 7.1)
{
CycleLEDs(0, false);
turnLEDOn(Green1);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.7 && (nImmediateBatteryLevel/1000) < 6.9)
{
CycleLEDs(0, false);
turnLEDOn(Green2);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.4 && (nImmediateBatteryLevel/1000) < 6.7)
{
CycleLEDs(0, false);
turnLEDOn(Yellow1);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.1 && (nImmediateBatteryLevel/1000) < 6.4)
{
CycleLEDs(0, false);
turnLEDOn(Yellow2);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 5.8 && (nImmediateBatteryLevel/1000) < 6.1)
{
CycleLEDs(0, false);
turnLEDOn(Red1);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 5.6 && (nImmediateBatteryLevel/1000) < 5.8)
{
CycleLEDs(0, false);
turnLEDOn(Red2);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 5.3 && (nImmediateBatteryLevel/1000) < 5.6)
{
CycleLEDs(0, false);
FlashLED(Red2,100);
}
else
{
CycleLEDs(0, false);
FlashLED(Red2,100);
FlashLED(Red1, 50);
}
}
// Switching
SwitchingDebugMenu();
// Test 2; main battery (avg)
clearLCDLine(0);
displayLCDCenteredString(0,"Avg M_Bat test");
while(SensorValue[DEBUG] != 1)
{
clearLCDLine(1);
displayLCDString(1, 0, "Average: ");
sprintf(LCD1, "%1.2f%c", (nAvgBatteryLevel/1000), 'V');
displayNextLCDString(LCD1);
if ((nAvgBatteryLevel/1000) >= 7.1)
{
CycleLEDs(0, false);
FlashLED(Green1, 100);
}
else if ((nAvgBatteryLevel/1000) >= 6.9 && (nAvgBatteryLevel/1000) < 7.1)
{
CycleLEDs(0, false);
turnLEDOn(Green1);
sleep(100);
}
else if ((nAvgBatteryLevel/1000) >= 6.7 && (nAvgBatteryLevel/1000) < 6.9)
{
CycleLEDs(0, false);
turnLEDOn(Green2);
sleep(100);
}
else if ((nAvgBatteryLevel/1000) >= 6.4 && (nAvgBatteryLevel/1000) < 6.7)
{
CycleLEDs(0, false);
turnLEDOn(Yellow1);
sleep(100);
}
else if ((nAvgBatteryLevel/1000) >= 6.1 && (nAvgBatteryLevel/1000) < 6.4)
{
CycleLEDs(0, false);
turnLEDOn(Yellow2);
sleep(100);
}
else if ((nAvgBatteryLevel/1000) >= 5.8 && (nAvgBatteryLevel/1000) < 6.1)
{
CycleLEDs(0, false);
turnLEDOn(Red1);
sleep(100);
}
else if ((nAvgBatteryLevel/1000) >= 5.6 && (nAvgBatteryLevel/1000) < 5.8)
{
CycleLEDs(0, false);
turnLEDOn(Red2);
sleep(100);
}
else if ((nAvgBatteryLevel/1000) >= 5.3 && (nAvgBatteryLevel/1000) < 5.6)
{
CycleLEDs(0, false);
FlashLED(Red2,100);
}
else
{
CycleLEDs(0, false);
FlashLED(Red2,100);
FlashLED(Red1, 50);
}
}
// Switching
SwitchingDebugMenu();
// Test 3: Load test
clearLCDLine(0);
displayLCDCenteredString(0,"M_Bat load test");
startTask(JoyStickControls);
while(SensorValue[DEBUG] != 1)
{
clearLCDLine(1);
displayLCDString(1, 0, "Immediate: ");
sprintf(LCD1, "%1.2f%c", (nImmediateBatteryLevel/1000), 'V');
displayNextLCDString(LCD1);
if ((nImmediateBatteryLevel/1000) >= 7.1)
{
CycleLEDs(0, false);
FlashLED(Green1, 100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.9 && (nImmediateBatteryLevel/1000) < 7.1)
{
CycleLEDs(0, false);
turnLEDOn(Green1);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.7 && (nImmediateBatteryLevel/1000) < 6.9)
{
CycleLEDs(0, false);
turnLEDOn(Green2);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.4 && (nImmediateBatteryLevel/1000) < 6.7)
{
CycleLEDs(0, false);
turnLEDOn(Yellow1);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 6.1 && (nImmediateBatteryLevel/1000) < 6.4)
{
CycleLEDs(0, false);
turnLEDOn(Yellow2);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 5.8 && (nImmediateBatteryLevel/1000) < 6.1)
{
CycleLEDs(0, false);
turnLEDOn(Red1);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 5.6 && (nImmediateBatteryLevel/1000) < 5.8)
{
CycleLEDs(0, false);
turnLEDOn(Red2);
sleep(100);
}
else if ((nImmediateBatteryLevel/1000) >= 5.3 && (nImmediateBatteryLevel/1000) < 5.6)
{
CycleLEDs(0, false);
FlashLED(Red2,100);
}
else
{
CycleLEDs(0, false);
FlashLED(Red2,100);
FlashLED(Red1, 50);
}
}
stopTask(JoyStickControls);
StopAllMotors();
//Swtiching
SwitchingDebugMenu();
// Test 4; backup battery
clearLCDLine(0);
displayLCDCenteredString(0,"Backup_Bat test");
while(SensorValue[DEBUG] != 1)
{
clearLCDLine(1);
displayLCDString(1, 0, "Backup: ");
sprintf(LCD1, "%1.2f%c", (BackupBatteryLevel/1000), 'V');
displayNextLCDString(LCD1);
if ((BackupBatteryLevel/1000) >= 9.2)
{
CycleLEDs(0, false);
FlashLED(Green1, 100);
}
else if ((BackupBatteryLevel/1000) >= 9 && (BackupBatteryLevel/1000) < 9.2)
{
CycleLEDs(0, false);
turnLEDOn(Green1);
sleep(100);
}
else if ((BackupBatteryLevel/1000) >= 8.8 && (BackupBatteryLevel/1000) < 9)
{
CycleLEDs(0, false);
turnLEDOn(Green2);
sleep(100);
}
else if ((BackupBatteryLevel/1000) >= 8.6 && (BackupBatteryLevel/1000) < 8.8)
{
CycleLEDs(0, false);
turnLEDOn(Yellow1);
sleep(100);
}
else if ((BackupBatteryLevel/1000) >= 8.4 && (BackupBatteryLevel/1000) < 8.6)
{
CycleLEDs(0, false);
turnLEDOn(Yellow2);
sleep(100);
}
else if ((BackupBatteryLevel/1000) >= 8.2 && (BackupBatteryLevel/1000) < 8.4)
{
CycleLEDs(0, false);
turnLEDOn(Red1);
sleep(100);
}
else if ((BackupBatteryLevel/1000) >= 8 && (BackupBatteryLevel/1000) < 8.2)
{
CycleLEDs(0, false);
turnLEDOn(Red2);
sleep(100);
}
else if ((BackupBatteryLevel/1000) < 8)
{
CycleLEDs(0, false);
FlashLED(Red2, 100);
}
}
// Switching
SwitchingDebugMenu();
// Test 5; joystick control
clearLCDLine(0);
clearLCDLine(1);
displayLCDCenteredString(0,"Remote test");
startTask(JoyStickControls);
while (SensorValue[DEBUG] != 1)
{
turnLEDOn(Green2);
FlashLED(Green1,250);
}
stopTask(JoyStickControls);
StopAllMotors();
// End of debug
displayLCDCenteredString(0,"End of debug");
clearLCDLine(1);
while (SensorValue[dgtl11] != 1)
{
CycleLEDs(DebugEndCounter);
CycleLEDs(DebugEndCounter, false);
DebugEndCounter = DebugEndCounter + 10;
}
}
// Normal operation
startTask(JoyStickControls);
Time = 0;
startTask(TimeKeeper); // Keeps up with time
while (1)
{
if (Time >= 105000) // Cycle lights (game over)
{
clearLCDLine(0);
clearLCDLine(1);
displayLCDCenteredString(0,"Time");
while (1)
{
CycleLEDs(DebugEndCounter);
CycleLEDs(DebugEndCounter, false);
DebugEndCounter = DebugEndCounter + 10;
}
}
else if (Time >= 95000) // Disable alerts (Don't ignore the driver)
{
clearLCDLine(0);
clearLCDLine(1);
displayLCDCenteredString(0,"Haul!");
displayLCDCenteredString(1,"Haul!");
CycleLEDs(0, false);
sleep(100);
}
else if (Time >= 75000 && Time < 95000) // Alert!! (30 seconds left)
{
displayLCDCenteredString(0,"Haul!");
clearLCDLine(1);
FlashLED(Red1, 100);
FlashLED(Red2, 100);
}
else // Normal operation
{
CycleLEDs(0, false);
CycleLEDs(100);
CycleLEDs(100, false);
FlashLED(Green1, 250);
FlashLED(Green2, 250);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment