void gen_code_build_stack()
{
int index = 0;
S_Function *target = TargetFunction(NULL);
target->code_offset = code_offset;
target->data_offset = data_offset;
target->param_offset = param_offset;
/* Regenerate argument data to reflect all data implementations */
gen_code_loc(0, DATA, GenerateValue(false, INT_T, data_offset-1));
memcpy(&target->code, &global_code, sizeof(Instruction)*MAXCODE);
}
void RobotMove::GoToPose(Pose newPose){
xDestination = newPose.getX();
yDestination = newPose.getY();
//Vector worldCoordinateVector(xDestination + (*position).GetXPos(), yDestination + (*position).GetYPos());
//Vector localCoordinateVector(worldCoordinateVector.getDistance(), dtor(worldCoordinateVector.getAngle()) + (*position).GetYaw(), false);
//xDestination = localCoordinateVector.getXIntensity();//x + (*position).GetXPos();
//yDestination = localCoordinateVector.getYIntensity();//y + (*position).GetYPos();
SensorScan sensors(&(*laser), &(*position), &(*worldView), sensorFunction, 1000.0);
double distance = closenessCutOff;
while (distance >= closenessCutOff) { //cout << "Setup";
(*robot).Read();
sensors.ReadSensors();
Vector sensorVector = sensors.VectorSum();
sensorVector.invertVector();
//sensorVector.debugIntensity();
Vector avoidVector(sensorVector.getDistance(), dtor(sensorVector.getAngle()) + (*position).GetYaw() , false);
Vector driveVector((*position).GetXPos(), (*position).GetYPos(), xDestination, yDestination);
distance = driveVector.getDistance();
TargetFunction(driveVector);
//TargetFunction(driveVector);
//cout << endl << "AvoidVector : ";
//avoidVector.debug();avoidVector.debugIntensity();
//cout << endl << "Drive Vector : ";
//cout << closenessCutOff << endl;
//driveVector.debug();
//driveVector.debugIntensity();
//cout << endl;
driveVector.add(avoidVector);
//cout << endl << "Resulting Vector : " << endl;
//driveVector.debug();driveVector.debugIntensity();
DriveToVector(driveVector, &(*position));
}
Vector directionVector(.1, newPose.getAngle(), true);
//while (abs((*position).GetYaw() - dtor(newPose.getAngle())) > closenessCutOff) {
// DriveToVector(directionVector, &(*position));
//}
(*position).SetSpeed(0,0);
}
void Breakaway10::TeleopContinuous(void)
{
// Called continuously while in the teleop part of the match.
// Each time the program returns from this function, it is immediately
// called again provided that the state hasn’t changed.
printf("TeleopContinuous -> Begin\b\n");
// Keep track of the previous joystick trigger value
bool lastTrigger = false;
// ->->-> MAIN LOOP BEGINS HERE <-<-<-
printf("TeleopContinuous -> Starting operator control loop\n");
//We need to start these tasks before we begin the loop, to use the enable function
ChaosKicker->Start();
double TargetDistance = 0.0; //cm
while(IsOperatorControl() && !IsDisabled())
{
TeleopLoopTimer->Start();
bool trigger;
GetWatchdog().Feed();
// if trigger is pulled, the robot will run with standard arcade drive
// otherwise the robot will home towards the target.
if (trigger = driverStick->GetTrigger())
{
dds->UpdateTimeIO = IO_UPDATE_TIME_SLOW;
TargetDistance = TargetFunction(trigger, lastTrigger);
DashData->TargetDistance_DashOut = TargetDistance;
ChaosKicker->DoSetExtendDistanceManual(false);
ChaosKicker->SetExtendDistance(GetTargetExtendDistance(TargetDistance));
}
else
{
dds->UpdateTimeIO = IO_UPDATE_TIME_NORMAL;
// if the trigger is not pressed, do not track
if (trigger != lastTrigger) turnController->Disable();
// Do tank drive now
base->TankDrive(driverStick->GetLeftY(), driverStick->GetRightY());
ChaosKicker->DoSetExtendDistanceManual(true);
}
//Send Trigger Data to Dashboard
lastTrigger = trigger;
DashData->Trigger_DashOut = trigger;
//Feed Watchdog Here
GetWatchdog().Feed();
// ->->-> KICKER SECTION BEGINS HERE <-<-<-
DashData->KickerExtendEistance_DashOut = ChaosKicker->ExtendDistance;
// END KICKER SECTION
// ->->-> DRIBBLER SECTION BEGINS HERE <-<-<-
//Both the left and right sides of the dribbler are equal, so only look for 2 pots, not 4
KLPot_Value = dseio.GetAnalogIn(KLPOT);
ALPot_Value = dseio.GetAnalogIn(ALPOT);
KLPot_Value = (KLPot_Value / 3.3) * 2.0;
KRPot_Value = KLPot_Value;
ALPot_Value = (ALPot_Value / 3.3);
ARPot_Value = ALPot_Value;
DashData->KLPot_Value_DashOut = KLPot_Value;
DashData->ALPot_Value_DashOut = ALPot_Value;
if(driverStick->GetNumberedButton(DRIBBLE_COMMAND))
{
dribbler->SetConstants(KLPot_Value, ALPot_Value, KRPot_Value, ARPot_Value);
dribbler->Keep();
}
else
{
dribbler->Release();
}
dribbler->BallIsInDribbler();
//END DRIBBLER SECTION
// ->->-> COMPRESSOR SECTION <-<-<-
DashData->PressureSwitch_DashOut = compressor->GetPressureSwitchValue();
// ->->-> KICKER STOW SETTING SECTION <-<-<-
ChaosKicker->SetWheelStowDistance(((dseio.GetAnalogIn(WHEEL_STOW_OFFSET_POT)/3.3)*(5))-2.5);
// ->->-> DASHBOARD DATA SENDER BEGINS HERE <-<-<-
dds->sendIOPortData();
// ->->-> LOOP TIME REGULATION SECTION <-<-<-
if(TeleopLoopTimer->Get() < TELEOP_PERIOD)
{
while(TELEOP_PERIOD - TeleopLoopTimer->Get() > 0)
{
GetWatchdog().Feed();
}
}
else
{
printf("Teleop Period NOT RESPECTED : CODE TAKES TOO LONG\n");
}
TeleopLoopTimer->Reset();
// END LOOP TIME REGULATION
} //END while Loop -> IsOperatorControl()
//If we leave the main loop (ie !IsEnabled) we need to also disable the tasks.
ChaosKicker->Stop();
} //End TeleopContinuous
void Breakaway10::AutonomousContinuous(void)
{
// Called continuously while the in the autonomous part of the match.
// Each time the program returns from this function, it is immediately
// called again provided that the state hasn’t changed.
printf("AutonomousContinuous\b\n");
compressor->Start();
//Find single ball using ultrasonic sensors
//The robot has ball in dribbler
AUTONOMOUS_STATES NextState = AUTO_STATE__BEGIN;
AUTONOMOUS_STATES CurrentState = AUTO_STATE__BEGIN;
GetWatchdog().Feed();
ChaosKicker->Start() ;
GetWatchdog().Feed();
AutonomousPot = dseio.GetAnalogIn(AUTO_EXTEND_POT_CHANNEL);
AutonomousExtendDistance = (AutonomousPot / 3.3) * 20;
KLPot_Value = dseio.GetAnalogIn(KLPOT);
ALPot_Value = dseio.GetAnalogIn(ALPOT);
KLPot_Value = (KLPot_Value / 3.3) * 2.0;
KRPot_Value = KLPot_Value;
ALPot_Value = (ALPot_Value / 3.3);
ARPot_Value = ALPot_Value;
DashData->KLPot_Value_DashOut = KLPot_Value;
DashData->ALPot_Value_DashOut = ALPot_Value;
DashData->AutoExtendDistance_DashOut = AutonomousExtendDistance;
while(IsEnabled() && IsAutonomous())
{
AutoLoopTimer->Start();
dribbler->SetConstants(KLPot_Value, ALPot_Value, KRPot_Value, ARPot_Value);
dribbler->BallIsInDribbler();
ChaosLogger->AutoStateEnum = CurrentState;
ChaosLogger->post();
switch(NextState)
{
case AUTO_STATE__BEGIN:
CurrentState = AUTO_STATE__BEGIN;
printf("AutoState -> BEGIN\n");
dribbler->Keep();
ChaosKicker->DoSetExtendDistanceManual(false);
ChaosKicker->SetExtendDistance(AutonomousExtendDistance);
NextState = AUTO_STATE__GET_BALL;
GetWatchdog().Feed();
break;
case AUTO_STATE__FIND_BALL:
CurrentState = AUTO_STATE__FIND_BALL;
Ball->ScanFor();
CurrentState = AUTO_STATE__FIND_BALL;
printf("AutoState -> FIND BALL\n");
if(Ball->IsFound() == SoccerBall::SCAN_SUCCESS__PARTIAL_SUCCESS || Ball->IsFound() == SoccerBall::SCAN_SUCCESS__FULL_SUCCESS)
{
Ball->StopScanFor();
NextState = AUTO_STATE__GET_BALL;
printf("NextState = AutoStateGetBall\n");
}
GetWatchdog().Feed();
break;
case AUTO_STATE__GET_BALL:
CurrentState = AUTO_STATE__GET_BALL;
printf("AutoState -> GET BALL\n");
//Ball->StopRetrieve();
Ball->GoRetrieve();
NextState = AUTO_STATE__HOLD_BALL;
GetWatchdog().Feed();
break;
case AUTO_STATE__HOLD_BALL:
printf("AutoState -> HOLD BALL\n");
if(CurrentState != NextState)
{
AutoSafeGuardTimer->Start();
CurrentState = AUTO_STATE__HOLD_BALL;
}
if(dribbler->BallIsInDribbler())
{
AutoSafeGuardTimer->Stop();
AutoSafeGuardTimer->Reset();
Ball->StopRetrieve();
//dribbler->Keep();
//NextState = AUTO_STATE__TARGET;
NextState = AUTO_STATE__PREPARE_FOR_KICK;
}
else if(!dribbler->BallIsInDribbler() && AutoSafeGuardTimer->Get() > SAFE_DRIBBLER_RETRIEVE_TIME)
{
AutoSafeGuardTimer->Stop();
AutoSafeGuardTimer->Reset();
//Added
Ball->StopRetrieve();
//dribbler->Keep();
NextState = AUTO_STATE__PREPARE_FOR_KICK;
}
else
{
//Keep Retrieving, but beware, you might go endlessly forward
//without the AutoSafeGuardTimer, which will throw the state mahcine
//Back into Get Ball to be safe
}
GetWatchdog().Feed();
break;
case AUTO_STATE__TARGET:
CurrentState = AUTO_STATE__TARGET;
if(CurrentState != AUTO_STATE__TARGET)
{
AutonomousTargetDistance = TargetFunction(true, false);
}
else
{
AutonomousTargetDistance = TargetFunction(true, true);
}
printf("AutoState -> TARGET\n");
if((TargetHorizontalAngle < (AUTO_HORIZONTAL_ANGLE_ALIGNED + HORIZONTAL_HYSTERISIS)) || (TargetHorizontalAngle > (AUTO_HORIZONTAL_ANGLE_ALIGNED - HORIZONTAL_HYSTERISIS)))
{
turnController->Disable();
NextState = AUTO_STATE__PREPARE_FOR_KICK;
}
GetWatchdog().Feed();
break;
case AUTO_STATE__PREPARE_FOR_KICK:
CurrentState = AUTO_STATE__PREPARE_FOR_KICK;
DashData->AutoTargetDistance_DashOut = AutonomousTargetDistance;
//ChaosKicker->SetExtendDistance(this->GetTargetExtendDistance(AutonomousTargetDistance));
//Use this when targeting
printf("AutoState -> PREPARE FOR KICK\n");
if(ChaosKicker->GetKickerStateMachineState() == Kicker::KICK_STATE__SIT_FOR_SHOOT)
{
NextState = AUTO_STATE__KICK_BALL;
}
else
{
NextState = AUTO_STATE__PREPARE_FOR_KICK;
//Wait for the dampner to be in the right position
}
GetWatchdog().Feed();
break;
case AUTO_STATE__KICK_BALL:
CurrentState = AUTO_STATE__KICK_BALL;
dribbler->Release();
AutoInterface->AutoKickerShoot = true;
printf("AutoState -> KICK BALL\n");
this->AutonomousWait(WAIT_FOR_WAIT_FOR_SHOOT_TIME);
if(ChaosKicker->GetKickerStateMachineState() == Kicker::KICK_STATE__SIT_FOR_SHOOT)
{
NextState = AUTO_STATE__BEGIN;
}
GetWatchdog().Feed();
break;
default:
NextState = AUTO_STATE__BEGIN;
GetWatchdog().Feed();
break;
}//End of switch
DashData->AutonomousCurrentState_DashOut = (int) CurrentState;
dds->sendIOPortData();
if(AutoLoopTimer->Get() < AUTO_PERIOD)
{
while(AUTO_PERIOD - AutoLoopTimer->Get() > 0.0)
{
GetWatchdog().Feed();
}
}
else
{
printf("Auto Period NOT RESPECTED : CODE TAKES TOO LONG\n");
}
AutoLoopTimer->Reset();
}
ChaosKicker->Stop();
ChaosLogger->closeIfOpen();
} // END AutonomousContinuous
