void autoRamp(bool useLift)
{
travelDistance(200, dBackward);
gyroTurn(30, 15, dRight);
goalRelease();
wait1Msec(100);
backward(15);
wait1Msec(1500);
stopMotors();
int iCRate = servoChangeRate[goalCapture]; // Save change rate
servoChangeRate[goalCapture] = 0; // Max Speed
servo[goalCapture] = CATCHDOWN; // Set servo position
wait1Msec(50);
servoChangeRate[goalCapture] = iCRate; // Reset the servo
wait1Msec(100);
travelDistance(10, dBackward);
strafe(75);
wait1Msec(500);
stopMotors();
travelDistance(100, dForward);
gyroTurn(30, 2, dLeft);
travelDistance(150, dForward);
gyroTurn(20, 100, dRight);
goalMed(useLift);
}
void forward( int rotations, int power ){
resetEncoders(); //Resets the motor encoder readings.
while( (nMotorEncoder[leftWheel1] < rotations) && (nMotorEncoder[rightWheel1] < rotations) && (nMotorEncoder[leftWheel2]) < rotations && (nMotorEncoder[rightWheel2] < rotations)){
showStatus(rotations, power);
motor[leftWheel1] = power;
motor[rightWheel1] = power;
motor[leftWheel2] = power;
motor[rightWheel2] = power;
}//goes forward until one of the two sides has rotated enough
stopMotors();
if( ( nMotorEncoder[leftWheel1] + nMotorEncoder[leftWheel2] ) - ( nMotorEncoder[rightWheel1] + nMotorEncoder[rightWheel2] ) > 100){
//if shifted to face left
while( nMotorEncoder[leftWheel1] - nMotorEncoder[rightWheel1] > 0 ){
motor[rightWheel1] = power / 5;//turns to be straight
}
stopMotors();
resetEncoders();
}
if( ( nMotorEncoder[rightWheel1] + nMotorEncoder[rightWheel2] ) - ( nMotorEncoder[leftWheel1] + nMotorEncoder[leftWheel2] ) > 100 ){
//if shifted to face right
while( nMotorEncoder[rightWheel1] - nMotorEncoder[leftWheel1] > 0 ){
motor[leftWheel1] = power/5;//turns to be straight
}
}
stopMotors();
resetEncoders();
}//forward function
void followLeftWall() {
/* Invoked when robot is driven and limit switches are activated.
** If the limit switches are activated then the robot is traveling
** in a direction that is somewhat close to being parrallel of the wall.
** We should correct our direction (to avoid a crash) and oscillate on the wall.
*/
writeDebugStreamLine("In followLeftWall");
while(SensorValue(frontRightBumper) == 0) {
driveMotors(-127,127);
wait1Msec(1000);
writeDebugStreamLine("In the first while loop of followLeftWall");
while ((SensorValue(leftLimitSensor) == 0) && (SensorValue(frontRightBumper) == 0)) {
writeDebugStreamLine("LEFT LIMIT SENSOR IS 0");
driveMotors(-127,127);
wait1Msec(200);
}
while ((SensorValue(leftLimitSensor) == 1) && (SensorValue(frontRightBumper) == 0) ) {
writeDebugStreamLine("LEFT LIMIT SENSOR IS 1");
stopMotors(300);
turnRight(1200);
driveMotors(-127,127);
wait1Msec(2000);
stopMotors(300);
turnLeft(1000);
}
driveMotors(-70,70);
wait1Msec(100);
}
}
task main()
{
gyroLeftTurn(90, 50);
stopMotors();
wait1Msec(1000);
gyroRightTurn(90, 50);
stopMotors();
}
void devicePwmMotorHandleRawData(char commandHeader, InputStream* inputStream, OutputStream* outputStream) {
if (commandHeader == COMMAND_MOVE_MOTOR) {
signed int left = readSignedHex2(inputStream);
signed int right = readSignedHex2(inputStream);
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_MOVE_MOTOR);
setMotorSpeeds(left * 2, right * 2);
}
else if (commandHeader == COMMAND_READ_MOTOR_VALUE) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_READ_MOTOR_VALUE);
signed int left = getLeftSpeed();
signed int right = getRightSpeed();
appendHex2(outputStream, left / 2);
appendHex2(outputStream, right / 2);
}
else if (commandHeader == COMMAND_STOP_MOTOR) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_STOP_MOTOR);
stopMotors();
}
else if (commandHeader == COMMAND_TEST_MOTOR) {
ackCommand(outputStream, MOTOR_DEVICE_HEADER, COMMAND_TEST_MOTOR);
appendString(getAlwaysOutputStreamLogger(), "Left Forward\n");
// Left forward
setMotorSpeeds(50, 0);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
// Right forward
setMotorSpeeds(0, 50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Left Backward\n");
// Left backward
setMotorSpeeds(-50, 0);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Right Forward\n");
// Right backward
setMotorSpeeds(0, -50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Both Forward\n");
// Both forward
setMotorSpeeds(50, 50);
delaymSec(2000);
appendString(getAlwaysOutputStreamLogger(), "Both Backward\n");
// Both backward
setMotorSpeeds(-50, -50);
delaymSec(2000);
stopMotors();
}
}
task main()
{
waitForStart();
stopMotors();
wait10Msec(500);
moveForward(2, 80);
wait10Msec(50);
rightTwoWheelTurn(10, 50);
wait10Msec(40);
//robot stops at first bucket from the right side of the pendulum
stopMotors();
wait10Msec(100);
/*
motor[tiltingMotor] = 75;
wait10Msec(105);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(200);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -45;
wait10Msec(110);
motor[tiltingMotor] = -25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);*/
armUp();
wait10Msec(200);
conveyorForward();
wait10Msec(200);
conveyorStop();
wait10Msec(100);
armDown();
wait10Msec(200);
moveForward(3, 80);
wait10Msec(50);
rightTwoWheelTurn(50, 50);
wait10Msec(83);
moveForward(25, 80);
wait10Msec(50);
leftTwoWheelTurn(48, 50);
wait10Msec(58);
moveForward(29, 80);
wait10Msec(50);
rightTwoWheelTurn(53, 50);
wait10Msec(150);
moveBackward(43, 80);
wait10Msec(60);
//robot parked in the middle of the ramp
}
task main()
{
waitForStart();
stopMotors();
wait10Msec(500);
moveForward(0.25, 80);
wait10Msec(50);
leftTwoWheelTurn(45, 50);
wait10Msec(56);
moveForward(41.5, 80);
wait10Msec(50);
rightTwoWheelTurn(45, 50);
wait10Msec(135);
//robot stopped in the third bucket from the right side of the pendulum
stopMotors();
wait10Msec(100);
motor[tiltingMotor] = 75;
wait10Msec(98);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(200);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -35;
wait10Msec(105);
motor[tiltingMotor] = -25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
leftTwoWheelTurn(42, 50);
wait10Msec(127);
moveForward(16, 80);
wait10Msec(50);
rightTwoWheelTurn(48, 50);
wait10Msec(53.5);
moveForward(25, 80);
wait10Msec(50);
rightTwoWheelTurn(48, 50);
wait10Msec(58);
moveForward(36, 80);
wait10Msec(50);
leftTwoWheelTurn(53, 50);
wait10Msec(115);
moveBackward(49, 80);
wait10Msec(50);
//robot parked in the middle of the ramp
}
void strafeRight(float tileDist)
{
/*
Moves Left X tiles
1 tile = 24in
15.7 = 1 Rotation (inches traveled)
24 / 15.7 = 1.528 = Rotation per tile
1 rotation = 240.448 counts
367.563 = Counts per tile
*/
float countGoal = tileDist * 367.563;
float currentCount = 0;
//Motors at full speed
writeDebugStreamLine("Right Strafe Started");
nMotorEncoder[backLeft] = 0; //Reset encoder count
currentCount = abs(nMotorEncoder[backLeft]);
motor[frontRight] = -127;
motor[frontLeft] = -127;
motor[backRight] = 127;
motor[backLeft] = 127;
while(currentCount < countGoal){
currentCount = abs(nMotorEncoder[backLeft]);
}
stopMotors();
writeDebugStreamLine("Strafe Right Ended");
}
task main()
{
bNxtLCDStatusDisplay = true;
HTMCsetTarget(HTMC);
while(HTMCreadRelativeHeading(HTMC)>-90){
motor[motorD] = 15;
motor[motorE] = 15;
motor[motorF] = 15;
motor[motorG] = 15;
}
stopMotors();
HTMCsetTarget(HTMC);
wait10Msec(10);
while(true){
checkBTLinkConnected();
readMultipleDataMsgs();
wait1Msec(1);
}
return;
}
task main()
{
disableDiagnosticsDisplay();
/*
motor[leftFront] = 50;
wait1Msec(1000);
motor[leftFront] = 0;
motor[rightFront] = 50;
wait1Msec(1000);
motor[rightFront] = 0;
motor[leftBack] = 50;
wait1Msec(1000);
motor[leftBack] = 0;
motor[rightBack] = 50;
wait1Msec(1000);
motor[rightBack] = 0;
*/
//travelDistance(45, dForward);
//gyroTurn(50, 35, dLeft);
forward(75);
wait1Msec(1000);
stopMotors();
displayCenteredTextLine(1, "lf = %i", nMotorEncoder[leftFront]);
displayCenteredTextLine(2, "lb = %i", nMotorEncoder[leftBack]);
displayCenteredTextLine(3, "rf = %i", nMotorEncoder[rightFront]);
displayCenteredTextLine(4, "rb = %i", nMotorEncoder[rightBack]);
wait1Msec(30000);
}
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr)
{
if (ptr) {
displayClear(pDisplay);
displayWrite(pDisplay, 5, 3, "REBOOTING...");
displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing?
stopMotors();
stopPwmAllMotors();
delay(200);
cmsTraverseGlobalExit(&menuMain);
if (currentMenu->onExit)
currentMenu->onExit((OSD_Entry *)NULL); // Forced exit
saveConfigAndNotify();
}
cmsInMenu = false;
displayRelease(pDisplay);
currentMenu = NULL;
if (ptr)
systemReset();
ENABLE_ARMING_FLAG(OK_TO_ARM);
return 0;
}
void gyroTurn(float power, float fDegrees, eDirection direct)
{//Robot turns at a specific power, a certain angle, either right or left
HTGYROstartCal(gyro); //Activate gyroscope
wait1Msec(100); //Let it adjust
float fCurrent = 0.0; //Set heading to 0 degrees
float fRotSpeed = 0.0; //Current rotational speed is 0
if (direct == dLeft) //If told to turn left
{
leftTurn(power); //Set motors to turn left
}
else //If tol to turn right
{
rightTurn(power); //Set motors to turn right
}
do //Loop through following until conditions are met
{
wait1Msec(MEASUREMENT_MS); //Give a moment to recalibrate
fRotSpeed = HTGYROreadRot(gyro); //Rotation speed is now the gyro's input
fCurrent += fRotSpeed * (MEASUREMENT_MS / 1000.0); //Current heading is what is was before
//plus the value of the rate of turn times the amount of time that rate value was taken
} while (abs(fCurrent) < fDegrees); //Repeat until the heading is the amount told to turn
stopMotors(); //You got there, now stop
}
void driveTime (int waitTime, int l, int r) //drive infinitely function, order: wait time, power s, power right
{
motor[motorD] = l;
motor[motorE] = r;
wait1Msec(waitTime);
stopMotors();
}
void setRobotPhysLimit( int distance, char direction, int height, int roller, int tray, int platform, int timer )
{
ClearTimer( T1 );
resetEncoders();
//artificiallyresetGyro();
while(time1[T1] < timer && !SensorValue[TowerLimitL] && !SensorValue[TowerLimitR] )
{
if( direction == 'S' )
setDrive( distance );
else if( direction == 'T' )
spinDrive( distance );
else
wheelDrive( distance, direction );
//setArm( height );
if( height == 0 )
moveArm( -10, -10 );
else
{
if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 1 )
moveArm( 7, 7 );
else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 )
moveArm( 7, 20 );
else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 )
moveArm( 20, 7 );
else
setArm( height );
}
moveRollers( roller, roller );
movePiston( tray, platform );
}
stopMotors();
}
void HardFault_Handler(void)
{
LED2_ON;
// fall out of the sky
uint8_t requiredStateForMotors = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredStateForMotors) == requiredStateForMotors) {
stopMotors();
}
#ifdef TRANSPONDER
// prevent IR LEDs from burning out.
uint8_t requiredStateForTransponder = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_TRANSPONDER_ENABLED;
if ((systemState & requiredStateForTransponder) == requiredStateForTransponder) {
transponderIrDisable();
}
#endif
LED1_OFF;
LED0_OFF;
while (1) {
#ifdef LED2
delay(50);
LED2_TOGGLE;
#endif
}
}
task main()
{
waitForStart();
moveForward(0.15, 70);
wait10Msec(50);
stopMotors();
wait10Msec(30);
rightTwoWheelTurn(45, 40);
wait10Msec(70);
stopMotors();
wait10Msec(30);
//robot moves backward, so it will be able to sense the ir beacon underneath first bucket
/*moveBackward(0.25, 80);
wait10Msec(50);*/
moveForward(6, 100);
wait10Msec(50);
stopMotors();
wait10Msec(30);
//keep going forward until ir sensor senses ir beacon
StartTask(irRightTesting);
wait10Msec(1400);
//wall follow the wall until the ultrasonic sensor stops sensing the black base underneath the pendulum
while (SensorValue[sonarSensor] < 80)
{
motor[frontLeftMotor] = 100;
motor[frontRightMotor] = -100;
motor[backRightMotor] = 100;
motor[backLeftMotor] = 100;
}
stopMotors();
wait10Msec(10);
moveForward(1.5, 90);
wait10Msec(50);
leftTwoWheelTurn(48, 85);
wait10Msec(40);
moveForward(30, 90);
wait10Msec(50);
leftTwoWheelTurn(42, 85);
wait10Msec(30);
moveForward(32, 90);
wait10Msec(50);
leftTwoWheelTurn(59, 85);
wait10Msec(70);
moveForward(44, 80);
wait10Msec(50);
//robot is parked in the middle of the ramp
}
// task that turns left when an IR seeker finds the IR beacon underneath a box to put the autonomous block
task irRightTesting()
{
while(true)
{
if(SensorValue(irSensor) == 5)
{
stopMotors();
wait10Msec(40);
moveForward(0.5, 80);
wait10Msec(100);
leftTwoWheelTurn(90, 80);
wait10Msec(98.5);
stopMotors();
wait10Msec(30);
motor[tiltingMotor] = 55;
wait10Msec(144);
motor[tiltingMotor] = 25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
motor[conveyorMotor] = 100;
wait10Msec(250);
motor[conveyorMotor] = 0;
wait10Msec(50);
motor[tiltingMotor] = -35;
wait10Msec(140);
motor[tiltingMotor] = -25;
wait10Msec(5);
motor[tiltingMotor] = 0;
wait10Msec(10);
stopMotors();
wait10Msec(100);
rightTwoWheelTurn(90, 40);
wait10Msec(186);
stopMotors();
wait10Msec(100);
break;
}
else
{
motor[frontLeftMotor] = 100;
motor[frontRightMotor] = -100;
motor[backRightMotor] = 100;
motor[backLeftMotor] = 100;
}
}
}
void autoFloor(bool useLift)
{
int irValue = getIRReading();
strafeDist(40, 100, dRight);
displayCenteredTextLine(1, "IR = %i", irValue);
if (irValue == 2)
{
strategyA(useLift);
}
else
{
irValue = getIRReading();
if (irValue == 2)
{
strategyA(useLift);
}
else
{
gyroTurn(10, 5, dRight);
int irValue1 = getIRReading();
gyroTurn(10, 10, dLeft);
int irValue2 = getIRReading();
eraseDisplay();
displayCenteredTextLine(2, "IR1 = %i", irValue1);
displayCenteredTextLine(3, "IR2 = %i", irValue2);
if (irValue1 == 2 || irValue2 == 2)
{
gyroTurn(10, 5, dRight);
strategyA(useLift);
}
else
{
strafeDist(40, 75, dRight);
gyroTurn(50, 30, dLeft);
resetEncoders();
while (irValue != 6)
{
irValue = getIRReading();
strafe(100);
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(3, "distance=%i", enc);
wait1Msec(1);
}
stopMotors();
int enc = abs(nMotorEncoder[leftBack]);
displayCenteredTextLine(1, "enc = %i", enc);
if (enc < 1500)
{
strategyB(useLift);
}
else
{
strategyC(useLift);
}
}
}
}
}
void HardFault_Handler(void)
{
// fall out of the sky
uint8_t requiredState = SYSTEM_STATE_CONFIG_LOADED | SYSTEM_STATE_MOTORS_READY;
if ((systemState & requiredState) == requiredState) {
stopMotors();
}
while (1);
}
//Start of Auto
task main()
{
waitForStart(); //waits for Starts
initializeRobot();
//Servos
armUp();
forward(6950, 50);
stopMotors();
forward(650, 20);
// drives down the ramp
armUp();
frontServoDown();
wait1Msec(500);
turnLeft(4700, 20);
wait1Msec(500);
//turns to face the other tube
armUp();
backServoUp();
wait1Msec(500);
backwards(2375, 15); //backwards for half the distance as we want
stopMotors();
backServoDown();
armDown();
PlaySound(soundBeepBeep);
wait1Msec(500);
armUp();
//scores balls
frontServoDown();
backServoDown();
wait1Msec(500);
turnRight(300, 30);
//backs into the second tube and grabs it
wait1Msec(500);
PlaySound(soundBeepBeep);
forward(9000, 50);
turnRight(300, 30);
forward(450, 50);
turnLeft(500, 30);
wait1Msec(500);
stopMotors();
PlaySound(soundBeepBeep);
forward(5000, 7.5); //forward(5000, 20); //makes sure we dont fall short.
}
void testLimitSwitch() {
wait1Msec(2000);
// Loop while robot's sensor is not pressed
while (SensorValue(leftLimitSensor) == 0 ) {
motor[leftMotor] = 23;
motor[rightMotor] = 23;
writeDebugStreamLine("Right Limit Sensor: %d", SensorValue(rightLimitSensor));
}
stopMotors(2000);
}
void strategyX() //use if in position 3, from ramp
{
displayCenteredTextLine(1, "strategy X"); //display whch strategy it chose
strafe(50); //strafe right at half power
wait1Msec(timeX1); //keep going for a certain amount of time
stopMotors(); //stop
travelDistance(distanceX1, dForward); //travel forward a certain distance
gyroTurn(30, 80, dLeft); //turn left 80 degrees
travelDistance(distanceX2, dForward); //travel forward a certain distance
}
task main(){
waitForStart();
servo[servo2] = 180;
motor[motorG] = 50;
motor[motorF] = 50;
sleep(850);
stopMotors(1);
straight(2, 50);
stopMotors(1);
goRight(95);
motor[motorG] = -50;
motor[motorF] = -50;
sleep(850);
straight(2.9, 50);
back(1.7, 50);
goRight(15);
straight(2.7, 50);
//Sandstorm();
}
void find_line()//finds white line on dark surface
{
int BLACK = SensorValue[S2];
while(SensorValue[S2] < (BLACK + 25))
{
nxtDisplayCenteredTextLine(3, "%i, %i", BLACK, SensorValue[S2]);
motor[left] = 45;
motor[right] = 45;
}
stopMotors();
}
void turnRight(int rotations, int power){
resetEncoders();//resets encoders
while( (nMotorEncoder[leftWheel2] < rotations) && abs(nMotorEncoder[rightWheel2]) < rotations ){
showStatus(rotations, power);
motor[rightWheel2] = 0;
motor[rightWheel1] = 0;
motor[leftWheel2] = power;
motor[leftWheel1] = power;
}
stopMotors();
}//turns right
/**
* Stop the robot.
*/
void stopPosition(PidMotion* pidMotion, bool maintainPositionValue, OutputStream* notificationOutputStream) {
updateTrajectoryAndClearCoders();
if (maintainPositionValue) {
maintainPosition(pidMotion, notificationOutputStream);
}
// Avoid that the robot considered he will remain the initial speed for next move (it is stopped).
clearInitialSpeeds(pidMotion);
DualHBridgeMotor* dualHBridgeMotor = pidMotion->dualHBridgeMotor;
stopMotors(dualHBridgeMotor);
}
task main()
{
int mSD, mSE, mSF, mSG;
runMotorSpeeds(mSD, mSE, mSF, mSG, 213, 25);
wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code
runMotorSpeeds(mSD, mSE, mSF, mSG, 270, 25);
wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code
runMotorSpeeds(mSD, mSE, mSF, mSG, 323, 25);
wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code
runMotorSpeeds(mSD, mSE, mSF, mSG, 270, 25);
wait1Msec(4000); // The program waits 4000 milliseconds (4 seconds) before running further code
stopMotors();
}
/**
* Stop the robot.
*/
void stopPosition(bool maintainPositionValue) {
updateTrajectoryAndClearCoders();
if (maintainPositionValue) {
maintainPosition();
} else {
// Avoid that robot reachs his position, and stops the motors
setMustReachPosition(false);
}
// Avoid that the robot considered he will remain the initial speed for next move (it is stopped).
clearInitialSpeeds();
stopMotors();
}
void strategyA(bool useLift)
{
displayCenteredTextLine(6, "Strategy A");
gyroTurn(30, 90, dLeft);
stopMotors();
wait1Msec(100);
travelDistance(40, dBackward);
wait1Msec(100);
strafeDist(27, 100, dLeft);
wait1Msec(100);
goalCenter(useLift);
strafeDist(45, 50, dLeft);
gyroTurn(30, 10, dLeft);
travelDistance(125, dBackward);
}
void examineID(msg_pointer mp){
printf("ID is %d\n", mp->ID);
if (mp->ID == START_ID)
startMotors();
if (mp->ID == STOP_ID)
stopMotors();
if (mp-> ID == CONTROL_ID)
controlMotors(mp);
if (mp-> ID == SPECIAL_COMMAND_ID)
specialMotorCommand(mp);
return;
}
