int main(void) {
setup();
int cycles = 2;
int cycle = 0;
bool direction = false;
uint32_t distance = 100;
uint16_t fSpeed = 60;
uint16_t rSpeed = 35;
while (1) {
if (cycles < 0 || cycle < cycles) {
if (!left_running && !right_running) {
delay();
direction = !direction;
if (direction) {
// leftWheel(direction, lSpeed, distance);
// rightWheel(direction, rSpeed, distance);
runCommand( FORWARD, fSpeed, distance / 2);
delay();
runCommand(LEFT_FWD, fSpeed, distance / 3);
delay();
runCommand( FORWARD, fSpeed, distance / 2);
delay();
runCommand(LEFT_FWD, fSpeed, distance / 3);
delay();
runCommand( FORWARD, fSpeed, distance / 2);
delay();
runCommand(LEFT_FWD, fSpeed, distance / 3);
delay();
runCommand( FORWARD, fSpeed, distance / 2);
delay();
allStop();
} else {
// leftWheel(direction, lSpeed / 2, distance);
// rightWheel(direction, rSpeed / 2, distance);
runCommand( REVERSE, rSpeed, distance);
delay();
runCommand(RIGHT_FWD, rSpeed, distance / 4);
delay();
runCommand( REVERSE, rSpeed, distance);
delay();
runCommand(RIGHT_FWD, rSpeed, distance / 4);
delay();
runCommand( REVERSE, rSpeed, distance);
delay();
runCommand(RIGHT_FWD, rSpeed, distance / 4);
delay();
runCommand( REVERSE, rSpeed, distance);
delay();
allStop();
}
cycle++;
}
}
}
}
int main(void) {
setup();
int cycles = 2;
int cycle = 0;
bool direction = false;
uint32_t distance = 1000;
uint16_t fSpeed = 80;
uint16_t rSpeed = 50;
while (1) {
if (cycles < 0 || cycle < cycles) {
if (!left_running && !right_running) {
delay();
direction = !direction;
if (direction) {
runCommand( FORWARD, fSpeed, distance);
allStop();
delay();
} else {
runCommand( REVERSE, rSpeed, distance);
allStop();
}
cycle++;
}
}
}
}
task main()
{
phase = AUTO;
driveCycles = 6;
humor = 80;
bDisplayDiagnostics = false;
initializeAutonomous();
bDisplayDiagnostics = true;
waitForStart();
motor [lift] = 100;
wait1Msec(750);
motor [lift] = 0;
wait1Msec(250);
while (nMotorEncoder [frontRight] > -(driveCycles * driveEncoderCycle))
{
drive(0, -25, 0);
}
while (SensorValue [sonar] > 50)
{
drive(0, 0, 25);
}
wait1Msec(550);
allStop();
StartTask(failSafe);
while (SensorValue [sonar] > 20)
{
drive(0, -20, 0);
}
StopTask(failSafe);
wait1Msec(500);
allStop();
servo [leftHook] = 168;
servo [rightHook] = 16;
wait1Msec(50);
score(60);
wait1Msec(200);
drive(0, 0, -100);
motor [arm] = 100;
servo [pivot] = 245;
wait1Msec(1000);
motor [arm] = 0;
wait1Msec(500);
allStop();
wait1Msec(50);
drive(-100, 0, 0);
wait1Msec(1700);
drive(0, -100, 0);
wait1Msec(4500);
allStop();
}
task main()
{
waitForStart();
init();
intakeOn(2);
raiseCube(4.5);
reverse(100,.7);
dropCube();
wait1Msec(1000);
cubeUp();
forward(100,.5);
right(100,1.5);
reverse(100,1);
left(100,1);
raiseArm(1);
reverse(100,.5);
left(100,1.5);
reverse(100,2);
allStop();
}
// If there is a radio packet, read and execute it. send info packets if appropriate
void DashBot::dashPacketHandler(void){
if(readRadioPacket()) {
executeRadioCommand();
}
debugBlinkOff();
//if in gyro-assisted mode, update controller and allow it to change the motor settings
if(mode == GYRO_MODE) {
dashRun(power, -heading);
}
//if in automatic info packet transmission mode is enabled, send an info packet
if(millis() - infoPacketTime > delay_between_sensor_emissions)
{
infoPacketTime = millis();
//if (infoPacketTransmissionMode == 1)
sendInfoPacket();
//else if (infoPacketTransmissionMode == 2)
// sendNamePacket();
//else if (infoPacketTransmissionMode == 3)
//sendMessagePacket();
}
//if there's not been a packet for the last 1/2 second, call an all-stop
if(millis() > lastPacketTime + 500)
{
allStop();
}
}
task main()
{
driveCycles = 6;
humor = 80;
bDisplayDiagnostics = false;
initializeAutonomous();
bDisplayDiagnostics = true;
waitForStart();
motor [lift] = 100;
wait1Msec(750);
motor [lift] = 0;
wait1Msec(250);
if (SensorValue [ir] == 5)
{
//while (nMotorEncoder [frontRight] > -(driveCycles * driveEncoderCycle))
//{
// drive(0, 25, 0);
//}
while (SensorValue [sonar] > 45)
{
drive(0, 100, 0);
}
allStop();
//begin precision rotate cycle
while (SensorValue [ir] > 4)
{
drive(0, 0, 60);
}
allStop();
ClearTimer(T2);
while (SensorValue [ir] < 6)
{
drive(0, 0, -60);
}
int spinTime = time1[T2];
allStop();
ClearTimer(T2);
while (time1[T2] < spinTime)
{
drive(0, 0, 60);
}
allStop();
}
}
void post3()
{
motor[motorJ]=-100;
motor[motorK]=-100;
wait10Msec(200);
allStop();
}
void Engines::disarm(){
if (!_armed) return;
setThrottle(0);
allStop();
_armed = false;
}
void post2()
{
motor[motorD]=-100;
motor[motorE]=100;
wait10Msec(60);
allStop();
}
task main() {
StartTask(USNEW);
StartTask(USOLD);
StartTask(pAxies);
MSSUMOsetShortRange(HTMSSUMO);
while(true)
{
runSensors();
nxtDisplayCenteredBigTextLine(1, "X: %d", xA);
nxtDisplayCenteredBigTextLine(3, "Y: %d", yA);
nxtDisplayCenteredBigTextLine(5, "Z: %d", zA);
zone = MSSUMOreadZone(HTMSSUMO);
switch (zone)
{
case MSSUMO_FRONT:
allStop();
break;
case MSSUMO_LEFT:
turnRight();
break;
case MSSUMO_RIGHT:
turnLeft();
break;
case MSSUMO_NONE:
allGo();
break;
}
if ((us_NewResult < 14 ) || ( us_OldResult < 14 ))
{
sonarObsticalCheck(1);
}
if ( xA < -25)
{
allStall();
wait1Msec(500);
allBack();
wait1Msec(1000);
sonarObsticalCheck(3);
}
if ( yA < -25)
{
allStall();
wait1Msec(500);
allBack();
wait1Msec(1000);
sonarObsticalCheck(3);
}
}
}
bool checkConnection()
{
nNoMessageCounterLimit = 250; //4ms per check (3 seconds after disconnect)
if(bDisconnected == 1)
{
allStop();
return 1;
}
return 0;
}
task raiseLiftWhile(){
raiseLift(100);
time1[T1]=0;
while (nMotorEncoder[intake] < 415 && time1[T1] < 2800) //while the encoder wheel turns one revolution
{
times = time1[T1];
}
allStop();
wait1Msec(500);
if(time1[T1]>2500)
StopAllTasks();
}
task main()
{
waitForStart();
forward(100,1);
init();
intakeOn(2);
left(100,.5);
raiseArm(1);
forward(100,1.8);
allStop();
}
task newi() //////////////////////////////////////THIS IS NEW STUFFFF.
{
//motorD drive
//motorE drive
//motorH lift
//motorI lift
//motorJ traverse
//motorK traverse
motor[motorD]=100;
motor[motorE]=100;
wait10Msec(#);
allStop();
motor[motorJ]=100;
motor[motorK]=100;
wait10Msec(#);
allStop();
motor[motorD]=100;
motor[motorE]=100;
wait10Msec(#);
allStop();
motor[motorD]=-100;
motor[motorE]=100;
allStop();
}
void Motors::setMotorSpeed(const byte motor, float speed)
{
//Mapping of the controller speed to the ESC speed
speed = (speed * 2) + MIN_MOTOR_SPEED_PWM - 6;
//If the speed command is too high we just shut down all the motors
//It might not be the best solution but it's at least safer for testing
if (speed > 260)
{
allStop();
Serial.print("Motor speed superior than 260. ALL MOTORS STOPPED");
while(1);
}
analogWrite(motors[motor-1], speed*motorsOn);
motor_speeds[motor-1] = speed;
}
MotorTest::MotorTest(QWidget *parent) : Page(parent)
{
int i;
setupUi(this);
m_cbobData = CbobData::instance();
QObject::connect(m_cbobData, SIGNAL(eStop()), this, SLOT(allStop()));
QObject::connect(ui_ClearButton, SIGNAL(pressed()), this, SLOT(resetMotorCounter()));
m_motorNumber = 0;
for(i=0;i<4;i++){
m_targetPower[i] = 0;
m_targetSpeed[i] = 0;
m_targetPosition[i] = 0;
m_controlState[i] = 2;
m_playState[i] = false;
}
}
task main(){
init(); //Initiate pieces
WaitForStart(); //Wait for the FCS to say go
StartTask(joystickOne); //Start joystick polling tasks
StartTask(joystickTwo);
while(true){ //Main execution loop
if(DEBUGMODE){DEBUG();}
if(bDisconnected){ //Stop Robot if disconnected
allStop();
continue;
}
updateSensors();
//Place any autonomous executions during teleop here
}
}
task main()
{
initializeRobot();
waitForStart();
//drive(0, 100, 0);
//wait1Msec(1000);
while (SensorValue [sonarSensor] > blockPlacementDist)
{
if (SensorValue [irSensor] < 5) {
drive(-50, 50, 0);
}
else if (SensorValue [irSensor] > 5) {
drive(50, 50, 0);
}
else if (SensorValue [irSensor] == 5) {
drive(0, 50, 0);
}
}
allStop();
placeBlock();
}
task main()
{
init();/*
float wait = 0.0;
while(nNxtButtonPressed!=1){ //Choose drive time
nxtDisplayCenteredTextLine(1,"Driving for:%fs",wait);
if(nNxtButtonPressed==0)
wait-=.5;
else if(nNxtButtonPressed==2)
wait+=.5;
if(wait<0)
wait=0;
}
*/
waitForStart();
forward(100);
wait1Msec(1500);
allStop();
wait1Msec(500);
}
void stopRobot(void) {
int sign;
float speed;
sign = sign(control.speeds.angular_speed);
speed = abs(control.speeds.angular_speed);
speed -= control.max_acc * DT;
speed = max(0, speed);
control.speeds.angular_speed = speed;
sign = sign(control.speeds.linear_speed);
speed = abs(control.speeds.linear_speed);
speed -= control.max_acc * DT;
speed = max(0, speed);
control.speeds.linear_speed = sign*speed;
if (abs(wheels_spd.left) + abs(wheels_spd.right) < SPD_TO_STOP) {
allStop();
} else {
applyPID();
}
}
//Lets Get this thing moving!
task main() {
// Start up the two sonar sensors and their config files
StartTask(USNEW);
StartTask(USOLD);
//Start up the IDMU config
StartTask(pAxies);
// Set Sumo to Long Range
MSSUMOsetShortRange(HTMSSUMO);
while(true) {
zone = MSSUMOreadZone(HTMSSUMO);
// Check front IR sensor LEFT FRONT RIGHT
switch (zone) {
case MSSUMO_FRONT:
allStop();
break;
case MSSUMO_LEFT:
turnRight();
break;
case MSSUMO_RIGHT:
turnLeft();
break;
case MSSUMO_NONE:
allGo();
break;
}
// Check left and right sonar sensors for objects
if ((us_NewResult < 12 ) || ( us_OldResult < 12 )){
sonarObsticalCheck(1);
} else if ( ( yA > 35 ) || ( xA > 35 ) ){
allStall();
wait1Msec(500);
allBack();
wait1Msec(1000);
sonarObsticalCheck(3);
//checkLevel();
}
}
}
YaesuRotor::YaesuRotor()
: rotorMoveUpdateInterval(100UL), rotatingAzimuth(false), rotatingElevation(false) {
// TODO check for saved config
// Rotor config defaults
config.configPresentFlag = 0xFF;
config.azimuthaAdZeroOffset = 0;
config.elevationAdZeroOffset = 0;
config.bias = 1 * SCALE_FACTOR;
// Initialise digital pins for output.
pinMode(PIN_EL_UP, OUTPUT);
pinMode(PIN_EL_DOWN, OUTPUT);
pinMode(PIN_AZ_LEFT, OUTPUT);
pinMode(PIN_AZ_RIGHT, OUTPUT);
allStop();
// Get current position and set target to same value to prevent unnecessary rotation on startup.
targetAzimuth = getCurrentAzimuth();
targetElevation = getCurrentElevation();
}
/**
Constructor establishes values for all motor pins
and sets all pins to output mode. allStop() is
called to prevent premature motion.
**/
HMotor::HMotor(){
rightSideMotor[0] = 7; // A side
rightSideMotor[1] = 8; // B side
rightSidePWM = 5;
leftSideMotor[0] = 4; // A side
leftSideMotor[1] = 9; // B side
leftSidePWM = 6;
rightSideCurrent = A2;
leftSideCurrent = A3;
rightSideEnable = A0;
leftSideEnable = A1;
pinMode(rightSideMotor[0], OUTPUT);
pinMode(rightSideMotor[1], OUTPUT);
pinMode(rightSidePWM, OUTPUT);
pinMode(leftSideMotor[0], OUTPUT);
pinMode(leftSideMotor[1], OUTPUT);
pinMode(leftSidePWM, OUTPUT);
allStop();
}
void _ISR _ADCInterrupt(void)
{
int i, thresholdMet = 1, threshold = 10000; // May need to adjust threshold
adcdata_t rawData[M_SLIDING_DFT_nchannels];
// TODO ADCBUF1, ADCBUF2, and ADCBUF3 may need to be swapped here
rawData[0] = ADCBUF1; // Copy ADC data
rawData[1] = ADCBUF2; // Copy ADC data
rawData[2] = ADCBUF3; // Copy ADC data
dft_update(dft, rawData); // Propagate the DFT
for (i = 0; i < M_SLIDING_DFT_nchannels; i ++) // For all channels:
thresholdMet &= (dft->mag[i] > threshold); // Check for threshold
if (thresholdMet) { // If we are hearing a ping:
float d10 = dft_relativePhase(dft, 0, 1); // TDOA 0/1
float d20 = dft_relativePhase(dft, 0, 2); // TDOA 0/2
steerToward(d10, d20); // Steer car
} else
allStop(); // Otherwise, halt car
IFS0bits.ADIF = 0; // Clear interrupt bit
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
servo[handJoint] = 240;
wait1Msec(100);
black_threshold = LSvalNorm(middle_light);
white_threshold = black_threshold + 5;
writeDebugStreamLine("black_threshold: %d", black_threshold);
writeDebugStreamLine("white_threshold: %d", white_threshold);
//
// Move to other side of field
//
moveStraight(50,2000);
move(-50,50,450);
moveStraight(50,500);
//
// Find the line
//
// Continue forward until the middle sensor detects the white line
//
writeDebugStreamLine("middle light sensor value going into first loop: %d" , LSvalNorm(middle_light));
while (LSvalNorm(middle_light) < white_threshold){
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
temp_middle_nrm = middle_nrm;
}
//--!
setAllMotorVals(10);
}
allStop();
wait1Msec(100);
//
// Turn 90 degrees
//
// Move forward a little more and then turn until the middle sensor detects the white line
//
bool isLeft = false; // If this boolean is true, the 90 degree turn is toward the left
moveStraight(30,350);
//moveStraight(15,1000); // with long arm
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter1 = 0;
while (LSvalNorm(middle_light) < white_threshold){
//!--Code for debugging
middle_nrm = LSvalNorm(middle_light);
if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("left light sensor value: %d", middle_nrm);
temp_middle_nrm = middle_nrm;
}
//--!
if (isLeft){
if (counter1 > 500){
setDriveMotorVals(40,-40);
}
else{
setDriveMotorVals(20,-20);
}
}
else{
if (counter1 > 100){
setDriveMotorVals(-40,40);
}
else{
setDriveMotorVals(-20,20); // with long arm
}
}
counter1 += 1;
}
//
// Follow the line until touch sensor is triggered
//
// 1. Left on white, middle on white (or black), right on black: turn toward the left
// 2. Left on black, middle on white (or black), right on white: turn toward the right
// 3. Left on black, middle on white, right on black: move straight
// 4. Left on black, middle on black, right on black: lost...
//
// Set up touch sensor variables
int nButtonsMask;
bool isTouch = false; // Boolean indicating if a touch sensor has been pressed.
// If one has, this changes to true.
// Make a variable that counts the number of times through the loop. If it goes above a certain
// threshold, assume the robot is stuck on the edge of the wood and increase the power
int counter2 = 0;
// Initial check of the touch sensor values. If a robot is pushed up against the spear,
// do not deploy it.
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01)
isTouch = true;
if (nButtonsMask & 0x02)
isTouch = true;
if (isTouch == false){
//deploy the spear
deploySpear(true);
}
while (isTouch == false){
writeDebugStreamLine("-----------------counter2 val: %d", counter2);
// Read light sensor values
left_nrm = LSvalNorm(left_light);
middle_nrm = LSvalNorm(middle_light);
right_nrm = LSvalNorm(right_light);
//!--Code for debugging
//if (left_nrm != temp_left_nrm) {
writeDebugStreamLine("left light sensor value: %d", left_nrm);
//temp_left_nrm = left_nrm;
//}
//if (middle_nrm != temp_middle_nrm) {
writeDebugStreamLine("middle light sensor value: %d", middle_nrm);
//temp_middle_nrm = middle_nrm;
//}
//if (right_nrm != temp_right_nrm) {
writeDebugStreamLine("middle light sensor value: %d", right_nrm);
//temp_right_nrm = right_nrm;
//}
//--!
if (left_nrm < black_threshold){
// left sensor is black
if (middle_nrm < black_threshold){
// middle sensor is black
if (right_nrm > black_threshold){
// right sensor is white or gray
// turn right
writeDebugStreamLine("Case 1: turned right");
if (counter2 > 600){
setDriveMotorVals(0,40);
}
else{
setDriveMotorVals(0,30);
}
}
else{
// right sensor is black
// lost, so just turn right a lot
setDriveMotorVals(-30,30);
}
}
else{
// middle sensor is not black
if (middle_nrm > white_threshold){
// middle sensor is white
// assume right sensor is black
// move straight
writeDebugStreamLine("Case 2: went straight");
if (counter2 > 600){
setAllMotorVals(40);
}
else{
setAllMotorVals(30);
}
}
else{
// middle sensor is gray
// assume right sensor is gray
// turn right
writeDebugStreamLine("Case 3: turned right");
if (counter2 > 600){
setDriveMotorVals(0,40);
}
else{
setDriveMotorVals(0,30);
}
}
}
}
else{
// left sensor is not black
if (left_nrm > white_threshold){
// left sensor is white
// assume middle sensor is black
// assume the right sensor is black too
// turn left
writeDebugStreamLine("Case 4: turned left");
if (counter2 > 600){
setDriveMotorVals(40,0);
}
else{
setDriveMotorVals(30,0);
}
}
else{
// left sensor is gray
// middle sensor is gray
// assume right sensor is black
// turn left
writeDebugStreamLine("Case 5: turned left");
if (counter2 > 600){
setDriveMotorVals(40,0);
}
else{
setDriveMotorVals(30,0);
}
}
}
counter2 += 1;
// Read touch sensor values
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01)
isTouch = true;
if (nButtonsMask & 0x02)
isTouch = true;
}
allStop();
wait1Msec(1000);
//
// More later....scoring stuff
//
// 1. Move backward a little
// 2. Unfold arm
// 3. Move forward a little
// 4. Move arm down to score
// 5. Back up
// 6. Reset arm
// 7. Move toward our rings?
//
// back up a little
moveStraight(-20,800);
// unfold arm
fold_arm(false);
// move forward a little
moveStraight(20,600);
// move arm down
motor[shoulderJoint] = -50;
wait1Msec(800);
motor[shoulderJoint] = 0;
// back up
moveStraight(-40,500);
// reset arm
fold_arm(true);
//retract spear
//deploySpear(false);
while (true){
return;
}
}
task main()
{
init();
waitForStart();
driveSonar(1,25,80);
allStop();
wait1Msec(100);
backward(20);
wait1Msec(700);
sticksDown();
wait1Msec(250);
allStop();
wait1Msec(100);
raiseLift(100);
time1[T1]=0;
while (nMotorEncoder[intake] < 410 && time1[T1] < 2500) //while the encoder wheel turns one revolution
{
times = time1[T1];
}
allStop();
wait1Msec(500);
//if(time1[T1]>2500)
// while(true){
// nxtDisplayCenteredBigTextLine(1,":(");
// }
releaseBalls();
allStop();
wait1Msec(3000);
retainBalls();
allStop();
wait1Msec(750);
lowerLift(80);
while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(500);
releaseAutoBall();
wait1Msec(500);
motor[intake] = 100;
wait1Msec(1000);
motor[intake] = 0;
turn(0,180,100);
allStop();
wait1Msec(100);
drive(1,1,75);
sticksUp();
drive(0,1,75);
time1[T1]=0;
while(SensorValue[sonarSensor]>30||time1[T1]<1200){
left(50);
}
while(SensorValue[sonarSensor]<200){
left(50);
}
allStop();
wait1Msec(100);
turn(0,25,70);
allStop();
wait1Msec(100);
driveSonar(1,25,50);
backward(30);
wait1Msec(700);
sticksDown();
wait1Msec(250);
allStop();
raiseLift(100);
while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(500);
releaseBalls();
allStop();
wait1Msec(3000);
retainBalls();
allStop();
wait1Msec(750);
lowerLift(80);
while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(500);
}
void Control_Process(void)
{
IsLight = (ADC_LeftIR + ADC_RightIR)/2 < ADC_Thumb;
if(!IsLight)
{
// "0123456789abcdef"
display_2 = " Dark ";
if(WasLight && (ProgramState == 1 || ProgramState == 3))
ProgramState++;
}
else
{
display_2 = " Light ";
}
if((ProgramState != 1 && ProgramState != 3) && MotorTimer <= 0)
ProgramState++;
if(OldState != ProgramState)
switch(ProgramState)
{
case 1:
allStop();
waitMsec(500);
MotorTimer = 10000;
LeftMotorPower = LPOWER;
RightMotorPower = RPOWER;
// "0123456789abcdef"
display_1 = "Forward to Line ";
break;
case 2:
allStop();
waitMsec(500);
LeftMotorPower = -LPOWER;
RightMotorPower = -RPOWER;
MotorTimer = 1000;
// "0123456789abcdef"
display_1 = " Blind Reverse ";
break;
case 3:
allStop();
MotorTimer = 10000;
LeftMotorPower = -LPOWER;
RightMotorPower = -RPOWER;
// "0123456789abcdef"
display_1 = "Reverse to Line ";
break;
case 4:
allStop();
//waitMsec(250);
MotorTimer = (10000 - MotorTimer + 4000)/2;
waitMsec(500);
LeftMotorPower = LPOWER-10;
RightMotorPower = RPOWER;
// "0123456789abcdef"
display_1 = "Forward to Mid ";
break;
case 5:
allStop();
waitMsec(500);
MotorTimer = SPIN_TIME*3;
LeftMotorPower = LPOWER;
RightMotorPower = -RPOWER;
// "0123456789abcdef"
display_1 = " Spin CW 3x ";
break;
case 6:
allStop();
waitMsec(500);
MotorTimer = SPIN_TIME*2;
LeftMotorPower = -LPOWER;
RightMotorPower = RPOWER;
// "0123456789abcdef"
display_1 = " Spin CCW 2x ";
break;
default:
allStop();
MotorTimer = 0;
// "0123456789abcdef"
display_1 = " Default ";
break;
}
WasLight = IsLight;
OldState = ProgramState;
}
task main()
{
init();
waitForStart();
StartTask(keepHeading);
StartTask(raiseLiftWhile);
driveSonar(1,25,50);
allStop();
wait1Msec(100);
backward(20);
wait1Msec(700);
sticksDown();
wait1Msec(250);
allStop();
wait1Msec(300);
//LIFT
releaseBalls();
allStop();
wait1Msec(3000);
retainBalls();
allStop();
wait1Msec(750);
lowerLift(80);
while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(100);
turn(1,19,70);
allStop();
wait1Msec(100);
drive(0,2.0,100);
allStop();
wait1Msec(100);
turn(0,170,100);
allStop();
wait1Msec(100);
sticksUp();
drive(1,.3,100);
allStop();
wait1Msec(100);
turn(0,180,100);
allStop();
wait1Msec(100);
//while(SensorValue[sonarSensor]>40){
// backward(100);
//}
//turnToGlobalHeading(-15);
allStop();
wait1Msec(100);
drive(1,1.3,100);
allStop();
wait1Msec(100);
turnToGlobalHeading(-78);
allStop();
wait1Msec(100);
while(SensorValue[sonarSensor]>25){
backward(30);
}
allStop();
wait1Msec(100);
while(SensorValue[sonarSensor]<200){
left(50);
}
allStop();
wait1Msec(100);
turn(0,18,100);
allStop();
wait1Msec(100);
driveSonar(1,25,50);
backward(30);
wait1Msec(700);
sticksDown();
wait1Msec(250);
allStop();
turn(1,17,70);
allStop();
wait1Msec(100);
drive(0,2.7,100);
allStop();
wait1Msec(100);
turn(0,175,100);
allStop();
wait1Msec(50);
sticksUp();
drive(1,.8,100);
}
void Control_Process(void)
{
IsLight = (ADC_LeftIR + ADC_RightIR)/2 < ADC_Thumb;
if(!IsLight)
{
// "0123456789abcdef"
display_2 = " Dark ";
}
else
{
display_2 = " Light ";
}
if(MotorTimer <= 0)
{
if(!IsLight)
ProgramState = 1;
if(IsLight)
ProgramState = 2;
}
if(ProgramState != OldState)
StateTransitions++;
/*if(StateTransitions == 50)
ProgramState = 3, StateTransitions++;
if(StateTransitions >= 100)
ProgramState = 0;
*/
if(MotorTimer <= 0)
{
switch(ProgramState)
{
case 1:
//allStop();
MotorTimer = 1;
LeftMotorPower = LPOWER;
RightMotorPower = -50;
// "0123456789abcdef"
display_1 = " Turning Right ";
break;
case 2:
//allStop();
MotorTimer = 1;
LeftMotorPower = -50;
RightMotorPower = RPOWER;
// "0123456789abcdef"
display_1 = " Turning Left ";
break;
case 3:
allStop();
MotorTimer = 1000;
LeftMotorPower = -LPOWER;
RightMotorPower = RPOWER;
default:
allStop();
//MotorTimer = 6000;
// "0123456789abcdef"
display_1 = " Default ";
break;
}
if(!IsLight)
ProgramState = 1;
if(IsLight)
ProgramState = 2;
}
WasLight = IsLight;
OldState = ProgramState;
}
task main()
{
init();
wait1Msec(1000);
sticksDown();
raiseLift(100);
time1[T1]=0;
while (nMotorEncoder[intake] < 430 && time1[T1] < 2500) //while the encoder wheel turns one revolution
{
times = time1[T1];
}
allStop();
wait1Msec(500);
//if(time1[T1]>2500)
// while(true){
// nxtDisplayCenteredBigTextLine(1,":(");
// }
releaseBalls();
allStop();
wait1Msec(3000);
retainBalls();
allStop();
wait1Msec(750);
lowerLift(20);
while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(500);
releaseAutoBall();
intakeIn(100);
wait1Msec(600);
allStop();
wait1Msec(500);
sticksUp();
wait1Msec(3000);
while(SensorValue[sonarSensor]>25){allStop();}
wait1Msec(2000);
sticksDown();
allStop();
wait1Msec(500);
raiseLift(100);
while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(500);
releaseBalls();
allStop();
wait1Msec(3000);
retainBalls();
allStop();
wait1Msec(750);
lowerLift(20);
while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution
{
}
allStop();
wait1Msec(500);
}