void Render(time_t t)
{
struct tm tm = *localtime(&t);
if (black)
SDL_SetRenderDrawColor(renderer, 0x00, 0x00, 0x00, 0);
else
SDL_SetRenderDrawColor(renderer, 0xFF, 0xFF, 0xFF, 0);
SDL_RenderClear(renderer);
// dial marks
if (black)
SDL_SetRenderDrawColor(renderer, 0xFF, 0xFF, 0x00, 0);
else
SDL_SetRenderDrawColor(renderer, 0x00, 0x00, 0xFF, 0);
for (int i = 0; i < 60; i++)
Mark(i);
// arms
if (black)
SDL_SetRenderDrawColor(renderer, 0x00, 0xFF, 0xFF, 0);
else
SDL_SetRenderDrawColor(renderer, 0x00, 0x00, 0x00, 0);
Arm(tm.tm_hour * 60 + tm.tm_min, 720, 8 * SHORT_MARK_LEN);
Arm(tm.tm_min, 60, 3 * SHORT_MARK_LEN);
SDL_SetRenderDrawColor(renderer, 0xFF, 0x00, 0x00, 0);
Arm(tm.tm_sec, 60, 2 * SHORT_MARK_LEN);
SDL_RenderPresent(renderer);
}
ArmMotionEngine::ArmMotionEngine() : ArmMotionEngineBase()
{
// initialize allMotions with default arm motion
allMotions.push_back(ArmMotion());
allMotions[0].id = ArmMotionRequestBH::useDefault;
allMotions[0].states.push_back(ArmMotion::ArmAngles());
allMotions[0].states[0].angles = std::vector<float>(4);
allMotions[0].states[0].hardness = std::vector<int>(4);
allMotions[0].states[0].steps = 40;
allMotions[0].states[0].angles[0] = -1.5707f;
allMotions[0].states[0].angles[1] = 0.2007f;
allMotions[0].states[0].angles[2] = -1.5707f;
allMotions[0].states[0].angles[3] = -0.2007f;
allMotions[0].states[0].hardness[0] = 80;
allMotions[0].states[0].hardness[1] = 80;
allMotions[0].states[0].hardness[2] = 80;
allMotions[0].states[0].hardness[3] = 80;
arms[ArmMotionRequestBH::left] = Arm(ArmMotionRequestBH::left, JointDataBH::LShoulderPitch);
arms[ArmMotionRequestBH::right] = Arm(ArmMotionRequestBH::right, JointDataBH::RShoulderPitch);
ASSERT(allMotions[ArmMotionRequestBH::useDefault].states.size() == 1);
defaultPos = allMotions[ArmMotionRequestBH::useDefault].states[0];
}
ArmMotionEngine::ArmMotionEngine() : ArmMotionEngineBase()
{
arms[ArmMotionRequest::left] = Arm(ArmMotionRequest::left, JointData::LShoulderPitch);
arms[ArmMotionRequest::right] = Arm(ArmMotionRequest::right, JointData::RShoulderPitch);
ASSERT(allMotions[ArmMotionRequest::useDefault].states.size() == 1);
defaultPos = allMotions[ArmMotionRequest::useDefault].states[0];
}
//--------------------------------------------
void CC4Projectile::Launch(const Vec3 &pos, const Vec3 &dir, const Vec3 &velocity, float speedScale)
{
CProjectile::Launch(pos, dir, velocity, speedScale);
if(!gEnv->bMultiplayer)
{
//don't want the hud grenade indicator on c4 in multiplayer
OnLaunch();
}
if(m_pAmmoParams->armTime > 0.f)
{
GetEntity()->SetTimer(ePTIMER_ACTIVATION, (int)(m_pAmmoParams->armTime*1000.f));
Arm(false);
}
else
{
Arm(true);
}
//Set up armed/disarmed materials and set material based on initial armed state
if(SC4ExplosiveParams* pExplosiveParams = m_pAmmoParams->pC4ExplosiveParams)
{
if(int count = GetEntity()->GetSlotCount())
{
for(int i = 0; i < count; i++)
{
SEntitySlotInfo info;
GetEntity()->GetSlotInfo(i, info);
if(info.pStatObj)
{
IMaterial* pMaterial = info.pStatObj->GetMaterial();
if(pMaterial)
{
m_pStatObj = info.pStatObj;
m_pStatObj->AddRef();
IMaterialManager* pMatManager = gEnv->p3DEngine->GetMaterialManager();
if( m_pArmedMaterial = pMatManager->LoadMaterial(pExplosiveParams->armedMaterial, false) )
{
m_pArmedMaterial->AddRef();
}
if( m_pDisarmedMaterial = pMatManager->LoadMaterial(pExplosiveParams->disarmedMaterial, false) )
{
m_pDisarmedMaterial->AddRef();
}
info.pStatObj->SetMaterial(m_armed ? m_pArmedMaterial : m_pDisarmedMaterial);
break;
}
}
}
}
}
}
// Picks up 4 rings
void PickUp4()
{
Arm(-127);
while(SensorValue[P1] + SensorValue[P2] > 400);
Trim();
Forward(30, 30);
Arm(-127);
while(SensorValue[P1] + SensorValue[P2] > 30);
wait1Msec(500);
Trim();
}
void Sprites::Arm(ULO spriteNo)
{
// Actually this is kind of wrong since each sprite serialises independently
bool is16Color = Is16Color(spriteNo);
if (is16Color && (spriteNo & 1) == 0) // Only arm the odd sprite
{
Arm(spriteNo + 1);
SpriteState[spriteNo].armed = false;
return;
}
SpriteState[spriteNo].armed = true;
SpriteState[spriteNo].x_cylinder = SpriteState[spriteNo].x;
SpriteState[spriteNo].pixels_output = 0;
SpriteState[spriteNo].dat_hold[0].w = SpriteState[spriteNo].dat[0];
SpriteState[spriteNo].dat_hold[1].w = SpriteState[spriteNo].dat[1];
if (is16Color)
{
SpriteState[spriteNo].dat_hold[2].w = SpriteState[spriteNo - 1].dat[0];
SpriteState[spriteNo].dat_hold[3].w = SpriteState[spriteNo - 1].dat[1];
Decode16(spriteNo);
}
else
{
Decode4(spriteNo);
}
}
// Moves the arm to wall height
void ArmWall()
{
ClearTimer(T1);
while(SensorValue[P1] + SensorValue[P2] < 2700 && time1[T1] < 4500)
{
Arm(127);
}
Trim();
}
// Moves the arm to base height
void ArmBase()
{
ClearTimer(T1);
while(SensorValue[P1] + SensorValue[P2] < 2212 && time1[T1] < 3000)
{
Arm(127);
}
Trim();
}
static void
MultiArm(Widget w, XEvent *event, String *params, Cardinal *num_params)
{
DEBUGOUT(_LtDebug(__FILE__, w, "MultiArm\n"));
if (DB_MultiClick(w) == XmMULTICLICK_KEEP)
{
Arm(w, event, NULL, NULL);
}
}
static void
MultiArm(
Widget aw,
XEvent *event,
String *params,
Cardinal *num_params)
{
if (((XmArrowButtonWidget) aw)->arrowbutton.multiClick == XmMULTICLICK_KEEP)
Arm(aw, event, params, num_params);
}
//--------------------------------------------
void CC4Projectile::ProcessEvent(SEntityEvent &event)
{
BaseClass::ProcessEvent(event);
if(event.event == ENTITY_EVENT_TIMER && event.nParam[0] == ePTIMER_ACTIVATION)
{
if(m_disarmTimer <= 0.f)
{
Arm(true);
}
}
}
/**
* Makes the kicker do the right things at the right time depending on what is happening.
*/
void Kicker::Act()
{
//Arm/Kick depending on the fire button on the kicker joystick
if (kickerJoystick->GetRawButton(joystickKickButton) == 1)
{
//If the kicker is in standby mode, who cares - life is over
//If the kicker is in arm mode, do nothing
//If the kicker is in kicking mode, let it continue
if (kickerMode == KICKER_MODE_ARMED)
{
//It's armed you say? Fire it baby!
kickerMode = KICKER_MODE_KICK;
}
} else {
//If the kicker is in armed mode, do nothing
//If the kicker is in arm mode, it's good to hear that it worked
//If the kicker is in kicking mode, let it continue
if (kickerMode == KICKER_MODE_STANDBY)
{
//If kicker is on standby, arm it
kickerMode = KICKER_MODE_ARM;
}
}
//Emergency armed button for those strange occations when things don't work right
if (kickerJoystick->GetRawButton(joystickEmergencyArmButton) == 1)
{
//Don't care where we are, just do it. I hate you.
kickerMode = KICKER_MODE_ARMED;
}
MoveRoller(rollerOn);
switch(kickerMode)
{
case KICKER_MODE_ARM:
Arm();
break;
case KICKER_MODE_KICK:
Kick();
break;
case KICKER_MODE_ARMED:
Armed();
break;
case KICKER_MODE_STANDBY:
default:
//Waiting for things to happen...
break;
}
}
//------------------------------------------------------------------------
bool CC4Projectile::NetSerialize(TSerialize ser, EEntityAspects aspect, uint8 profile, int pflags)
{
if(aspect == ASPECT_C4_STATUS)
{
bool isArmed = m_armed;
ser.Value("m_armed", isArmed, 'bool');
if(ser.IsReading())
{
Arm(isArmed);
}
}
m_stickyProjectile.NetSerialize(this, ser, aspect);
return BaseClass::NetSerialize(ser, aspect, profile, pflags);
}
CC4Projectile::CC4Projectile():
m_armed(false),
m_teamId(0),
m_disarmTimer(0.f),
m_pulseTimer(0.f),
m_pStatObj(NULL),
m_pArmedMaterial(NULL),
m_pDisarmedMaterial(NULL),
m_pLightSource(NULL),
m_OnSameTeam(false),
m_stickyProjectile(true),
m_isShowingUIIcon(false)
{
Arm(false);
}
void CC4Projectile::Update(SEntityUpdateContext &ctx, int updateSlot)
{
if(gEnv->bMultiplayer)
{
IActor* pActor = g_pGame->GetIGameFramework()->GetIActorSystem()->GetActor(m_ownerId);
if(!pActor || pActor->IsDead())
{
if(gEnv->bServer)
{
Destroy();
}
else
{
GetEntity()->Hide(true);
}
RemoveLight();
if(m_isShowingUIIcon)
{
SHUDEvent hudEvent(eHUDEvent_RemoveC4Icon);
hudEvent.AddData((int)GetEntityId());
CHUDEventDispatcher::CallEvent(hudEvent);
m_isShowingUIIcon = false;
}
}
}
if(m_pLightSource)
{
UpdateLight(ctx.fFrameTime, false);
}
if(m_disarmTimer > 0.f)
{
m_disarmTimer -= ctx.fFrameTime;
if(m_disarmTimer <= 0.f)
{
m_disarmTimer = 0.f;
Arm(true);
}
}
BaseClass::Update(ctx, updateSlot);
}
static void
ArmAndActivate(Widget w, XEvent *event, String *params, Cardinal *num_params)
{
XmDrawnButtonCallbackStruct cbs;
DEBUGOUT(_LtDebug(__FILE__, w, "ArmAndActivate\n"));
/* Arm, Activate, and Disarm now, but draw the disarmed state later */
Arm(w, event, params, num_params);
if (!Lab_SkipCallback(w) && DB_ActivateCallback(w))
{
XFlush(XtDisplay(w));
cbs.reason = XmCR_ACTIVATE;
cbs.event = event;
cbs.window = XtWindow(w);
cbs.click_count = 1;
XtCallCallbackList(w,
DB_ActivateCallback(w),
(XtPointer)&cbs);
}
DB_Armed(w) = False;
if (DB_DisarmCallback(w))
{
XFlush(XtDisplay(w));
cbs.reason = XmCR_DISARM;
cbs.event = event;
cbs.window = XtWindow(w);
cbs.click_count = 1;
XtCallCallbackList(w,
DB_DisarmCallback(w),
(XtPointer)&cbs);
}
if (DB_Timer(w) != 0)
{
XtRemoveTimeOut(DB_Timer(w));
DB_Timer(w) = 0;
}
DB_Timer(w) = XtAppAddTimeOut(XtWidgetToApplicationContext(w),
ACTIVATE_DELAY, ArmTimeout, (XtPointer)w);
}
task main()
{
waitForStart();
while(true)
{
getJoystickSettings(joystick);
Drive(Left, Right); // joystick1 drive
Arm(Lift); // joystick2 arm
Spin(); // joystick2 basket spin
FlagSpin(); // joystick2 and 1 flag
AtonAarm(); // joystick2 aton arm
Wrist(); // joystick2 wrist
Assist(assist);
if (joy2Btn(10)) // arm speed control
{ // starts slow
i = i + 1;
wait1Msec(500);
}
}
}
Robot::Robot(double x, double y, double a, QObject *parent){
md_linearVelocity=md_angularVelocity=0;
md_wheelTrack=1.0;
for (int i=0; i<2; ++i)
m_wheel[i] = Wheel(0,0.3);
md_position = new double [3];
md_position[0]=x;
md_position[1]=y;
md_position[2]=a;
m_cylinder = Cylinder();
m_arm = Arm();
battery = new Battery();
state=BASIC;
socket = new QTcpSocket(this);
socket->connectToHost(QString("192.168.0.1"), 2000);
connect(socket, SIGNAL(connected()), this, SLOT(connected()));
connect(socket, SIGNAL(hostFound()), this, SLOT(connected()));
connect(socket, SIGNAL(disconnected()), this, SLOT(not_connected()));
bl=new char[4];
}
void vTaskCommander (void *pvParameters)
{
QueueSetMemberHandle_t xActivatedMember;
RadioLinkData radioData;
IMUData imuData;
LogData log;
CommanderData commanderData;
static bool isArmed = false;
static float lastTick = xTaskGetTickCount();
//(const float kp, const float ki, const float kd, const float intLimit)
PidObject yawRatePid(0.3f, 0, 0.1f, 1);
PidObject pitchPid (2.0f, 10.0f, 0.13f, 10.0f);
PidObject rollPid (2.0f, 10.0f, 0.13f, 10.0f);
static bool imuReady = false; // We need at last one IMU data packet to start work.
static bool radioReady = false; // We need at last one radio data packet to start work.
while(1)
{
xActivatedMember = xQueueSelectFromSet(TheStabilizerQueueSet, 10);
if (xActivatedMember == TheRadioCommandsQueue)
{
xQueueReceive(TheRadioCommandsQueue, &radioData, 0 );
radioReady = true;
}
if (xActivatedMember == TheIMUDataQueue)
{
xQueueReceive(TheIMUDataQueue, &imuData, 0 );
imuReady = true;
}
else
{
// failure!
continue;
}
if (!imuReady || !radioReady)
{
// Wait for first complete data.
continue;
}
if (!isArmed)
{
// We can arm only if IMU and radio ready.
if (Arm(commanderData))
{
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
continue;
}
// Arm done.
isArmed = true;
}
TheLedInfo->Y(true);
// dT calculation
const float currentTick = xTaskGetTickCount();
const float dT = (currentTick - lastTick) / 1000.0f; // Seconds
lastTick = currentTick;
if (dT == 0)
{
continue; // Skip first iteration.
}
if (radioData.Throttle <= 2 )
{
yawRatePid.Reset();
pitchPid.Reset();
rollPid.Reset();
commanderData.Throttle = 0;
commanderData.Pitch = 0;
commanderData.Roll = 0;
commanderData.Yaw = 0;
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
continue;
}
// Convert radio to angles
const float angle_max = radians(30.0f);
const float yawRateDesired = map(radioData.Yaw, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float pitchDesired = -map(radioData.Pitch, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float rollDesired = map(radioData.Roll, -100.0f, 100.0f, -angle_max, angle_max); // [-angle_max, angle_max] radians
const float throttleDesired = map(radioData.Throttle, 0.0f, 100.0f, 10.0f, 100.0f); // [10, 100] percent of motor power
// Feed PID regulators
const float yawCorrection = yawRatePid.Update( yawRateDesired, imuData.Yaw, dT, true); // radians
const float pitchCorrection = pitchPid .Update( pitchDesired, imuData.Pitch, dT, true); // radians
const float rollCorrection = rollPid .Update( rollDesired, imuData.Roll, dT, true); // radians
// Calculate motors power
const float pm = 30.0f * throttleDesired / 100.0f; // Multiplier for Pitch
const float rm = 30.0f * throttleDesired / 100.0f; // Multiplier for Roll
const float ym = 5.0f; // Multiplier for Yaw
const float pitchK = pitchCorrection * pm; // [-1, 1] pitch correction
const float rollK = rollCorrection * rm; // [-1, 1] roll correction
const float yawK = cosf(yawCorrection); // [-1, 1] yaw correction
commanderData.Throttle = throttleDesired;
commanderData.Pitch = pitchK;
commanderData.Roll = rollK;
commanderData.Yaw = yawK;
xQueueOverwrite( TheCommanderDataQueue, &commanderData );
TheGlobalData.DBG_PID_YAW = yawCorrection;
TheGlobalData.DBG_PID_PITCH = pitchCorrection;
TheGlobalData.DBG_PID_ROLL = rollCorrection;
TheGlobalData.DBG_CO_YAW = yawK;
TheGlobalData.DBG_CO_PITCH = pitchK;
TheGlobalData.DBG_CO_ROLL = rollK;
TheLedInfo->Y(false);
/*
log.Timer = xTaskGetTickCount();
log.InputThrottle = radioData.Throttle;
log.InputYaw = radioData.Yaw;
log.InputPitch = radioData.Pitch;
log.InputRoll = radioData.Roll;
log.Yaw = imuData.EulierAngles.Z;
log.Roll = imuData.EulierAngles.Y;
log.Pitch = imuData.EulierAngles.X;
*/
// Testing telemetry.
//xQueueOverwrite( TheLogQueue, &log );
// Process Data!
}
vTaskDelete(NULL);
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
int IRvalue = SensorValue[IR];
wait10Msec(170);
//Start();
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
if(IRvalue==5)
{
Move(12.0, 70.0); // drive forward 12 inches
wait10Msec(10);
Turn_gyro(60.0); // turn counterclockwise 60 degrees
wait10Msec(10);
Move(34.0, 70.0); // drive forward 34 inches
wait10Msec(10);
Arm(205.0, 70.0); // arm move 90 degrees
wait10Msec(100);
Move(14.0, 70.0); // drive forward 14 inches
wait10Msec(10);
Arm(25.0, 70.0); // arm move 90 degrees
wait10Msec(100);
Move(-4.0, 70.0); // drive backward 4 inches
wait10Msec(10);
}
if(IRvalue==6 || IRvalue==0)
{
Move(22.0, 70.0); // drive forward 22 inches
wait10Msec(10);
Turn_gyro(45.0); // turn counterclockwise 45 degrees
wait10Msec(10);
Move(20.0, 70.0); // drive forward 20 inches
wait10Msec(10);
Arm(197.0, 70.0); // arm move 197 degrees
wait10Msec(100);
Move(14.0, 70.0); // drive forward 14 inches
wait10Msec(10);
Arm(40.0, 70.0); // arm move 40 degrees
wait10Msec(100);
Move(-4.0, 70.0); // drive backward 48 inches
wait10Msec(10);
}
if(IRvalue==7)
{
}
}
// This function is never called if there is a calibration issue.
// called in true section of if (RX_Snapshot[RXChannel_THR] < STICKThrottle_ARMING)
void HandleSticksForArming (void)
{
if ((UIEnableStickCommands==false) || (ActiveRXIndex!=1) || (!IS_TX2_GOOD)) return ; // you cannot use Primary to Arm and Disarm
SystemErrorType = CLR_SYS_ERR_SIGNAL;
if (TCNT1_X_snapshot1==0) TCNT1_X_snapshot1 = CurrentTCNT1_X; // start counting
// DisArm Check
if (IsArmed == true)
{
if (RX_Latest[ActiveRXIndex][RXChannel_RUD] > STICK_LEFT)
{ // Check DisArming manually.
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X - TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Disarm();
return ;
}
}
if (Config.AutoDisarm!=0)
{ // check auto disArm
if (TCNT_X_snapshotAutoDisarm > CurrentTCNT1_X ) TCNT_X_snapshotAutoDisarm = 0; // the CurrentTCNT1_X was high so the disarm condition never true.
if (TCNT_X_snapshotAutoDisarm==0) TCNT_X_snapshotAutoDisarm = CurrentTCNT1_X;
if ((CurrentTCNT1_X - TCNT_X_snapshotAutoDisarm) > (DISARM_TIME * Config.AutoDisarm))
{
Disarm();
return ;
}
}
}
if (IsArmed == false)
{
//int16_t Stick = (Config.RX_Mid[ActiveRXIndex][RXChannel_RUD] + RX_Latest[ActiveRXIndex][RXChannel_RUD] * RX_Div_Factor);
if (RX_Latest[ActiveRXIndex][RXChannel_RUD] < STICK_RIGHT)
{ // Armin Check
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Arm();
return ;
}
}
//set modes Quad , X-Quad
if (RX_Latest[ActiveRXIndex][RXChannel_AIL] > STICK_LEFT)
{// X-QUAD MODE
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Config.QuadFlyingMode=QuadFlyingMode_X;
LED_FlashOrangeLED (LED_LONG_TOGGLE,8);
TCNT1_X_snapshot1 =0; // reset timer
}
}
else if ((RX_Latest[ActiveRXIndex][RXChannel_AIL] < STICK_RIGHT))
{ // QUAD COPTER MODE
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
Config.QuadFlyingMode=QuadFlyingMode_PLUS;
LED_FlashOrangeLED (LED_LONG_TOGGLE,4);
TCNT1_X_snapshot1 =0; // reset timer
}
}
// set mode ACRO , Leveling
else if ((RX_Latest[ActiveRXIndex][RXChannel_ELE] < STICK_RIGHT))
{ //nFlyingModes = FLYINGMODE_LEVEL;
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
nFlyingModes = FLYINGMODE_LEVEL;
LED_FlashOrangeLED (LED_LONG_TOGGLE,8);
TCNT1_X_snapshot1 =0; // reset timer
}
}
else if ((RX_Latest[ActiveRXIndex][RXChannel_ELE] > STICK_LEFT))
{ //nFlyingModes = FLYINGMODE_ACRO;
bResetTCNR1_X = false;
if ( (CurrentTCNT1_X- TCNT1_X_snapshot1) > STICKPOSITION_LONG_TIME )
{
nFlyingModes = FLYINGMODE_ACRO;
LED_FlashOrangeLED (LED_LONG_TOGGLE,4);
TCNT1_X_snapshot1 =0; // reset timer
}
}
}
}
void KeyControlledAnimatedCharacter_cl::ProcessKeyboardEvents()
{
bool bLeft = m_pInputMap->GetTrigger(CHARACTER_MOVE_LEFT)!=0;
bool bRight = m_pInputMap->GetTrigger(CHARACTER_MOVE_RIGHT)!=0;
bool bUp = m_pInputMap->GetTrigger(CHARACTER_MOVE_FORWARD)!=0;
bool bDown = m_pInputMap->GetTrigger(CHARACTER_MOVE_BACKWARD)!=0;
bool bShift = m_pInputMap->GetTrigger(CHARACTER_RUN)!=0;
bool bRaiseLowerWeapon = m_pInputMap->GetTrigger(CHARACTER_TOGGLE_WEAPON)!=0;
bool bAttack = m_pInputMap->GetTrigger(CHARACTER_ATTACK)!=0;
if (bUp)
{
// trigger the run/walk actions when CURSOR UP is pressed.
// allow rotating the entity while walking/running
if ( bShift )
Run();
else
Walk();
if (bLeft)
RotateLeft();
else if (bRight)
RotateRight();
}
else if(bDown)
{
// trigger the walk backward action when CURSOR DOWN is pressed.
// allow rotating the entity while walking backwards
WalkBackwards();
if(bLeft)
RotateLeft();
else if(bRight)
RotateRight();
}
else
{
// switch to stand action if character is not rotating ???????????
if ( !IsInRotateState() && !IsInRotateArmedState() )
Stand();
// trigger rotate actions if CURSOR LEFT/RIGHT is pressed, otherwise trigger stand action
if(bLeft)
RotateLeft();
else if(bRight)
RotateRight();
else
Stand();
}
// raise or lower weapon on SPACE key
if(bRaiseLowerWeapon)
{
if(IsInUpperBodyIdleState()||IsInUpperBodyLowerWeaponState())
Arm();
else if(IsInUpperBodyIdleArmedState()||IsInUpperBodyDrawWeaponState())
DisArm();
}
// attack on ENTER key
if ( bAttack )
GetUpperBodyState()->Attack();
}
task usercontrol()
{
// Autonomous trigger in drive control
if(vexRT[Btn5D] && vexRT[Btn5U] && vexRT[Btn6D] && vexRT[Btn6U])
{
StartTask(autonomous);
wait1Msec(20000);
StopTask(autonomous);
}
if(!Out)
{
StartTask(descore);
}
float multiplier = 1;
bool wall = false;
bool base = false;
bool down = false;
bool macro = false;
int id = 0;
int step = 0;
while(true)
{
Drive(multiplier);
if(vexRT[Btn7U])
{
wall = true;
base = false;
down = false;
macro = false;
}
else if(vexRT[Btn7D])
{
wall = false;
base = true;
down = false;
macro = false;
}
else if(vexRT[Btn7L])
{
wall = false;
base = false;
down = true;
macro = false;
}
if(vexRT[Ch2] > 20 || vexRT[Ch2] < -20)
{
Arm();
wall = false;
base = false;
down = false;
macro = false;
}
else if(wall)
{
if(SensorValue[P1] + SensorValue[P2] != 2891)
{
Arm(127);
}
else
{
wall = false;
}
}
/*
else if(base)
{
if(SensorValue[P1] + SensorValue[P2] != 2212)
{
Arm(271 - (SensorValue[P1] + SensorValue[P2]) / 10);
}
else
{
base = false;
}
}
*/
else if(down)
{
if(SensorValue[P1] + SensorValue[P2] > 12)
{
Arm(-127);
}
else
{
down = false;
}
}
else if(!macro)
{
Trim();
}
if(vexRT[Btn5U])
{
macro = false;
Down();
}
else if(!macro)
{
Up();
}
if(vexRT[Btn8D])
{
multiplier = 0.25;
}
else if(vexRT[Btn8L])
{
multiplier = 0.5;
}
else if(vexRT[Btn8U])
{
multiplier = 0.75;
}
else if(vexRT[Btn8R])
{
multiplier = 1;
}
if(vexRT[Btn6U])
{
StartTask(descore);
}
/*
if(macro)
{
// macro id's
// 1 = pick up 4
switch (id)
{
case 1:
switch(step)
{
case 0:
if(SensorValue[P1] + SensorValue[P2] > 400)
{
Arm(-127);
}
else
{
Trim();
++step;
}
break;
case 1:
if(SensorValue[QuadLeft] < 40 || SensorValue[QuadRight] < 40)
{
if(SensorValue[QuadLeft] < 40)
{
DriveLeft(30);
}
else
{
LeftBrake();
}
if(SensorValue[QuadRight] < 40)
{
DriveRight(30);
}
else
{
RightBrake();
}
}
else
{
Stop();
ClearTimer(T1);
++step;
}
break;
case 3:
if(time1[T1] < 500)
{
Arm(-127);
}
else
{
ClearTimer(T1);
++step;
}
break;
case 4:
if(SensorValue[P1] + SensorValue[P2] < 2212 || time1(T1) < 1000) //2060 -> 2212
{
Arm(271 - (SensorValue[P1] + SensorValue[P2]) / 10);
}
else
{
Trim();
macro = false;
id = 0;
}
break;
}
break;
case 2:
switch(step)
{
case 0:
if(GetRingCount() > 0)
{
Down();
}
else
{
++step;
ClearTimer(T1);
}
break;
case 1:
if(time1[T1] < 1000)
{
Down();
}
else
{
Up();
macro = false;
id = 0;
}
break;
}
break;
}
}
if(vexRT[Btn6U])
{
wall = false;
base = false;
down = false;
macro = true;
SensorValue[QuadCenter] = 0;
SensorValue[QuadLeft] = 0;
SensorValue[QuadRight] = 0;
if(GetRingCount() < 3 && SensorValue[P1] + SensorValue[P2] > 1500 && SensorValue[P1] + SensorValue[P2] < 2212)
{
id = 1;
}
else if(GetRingCount() > 0 && SensorValue[P1] + SensorValue[P2] >= 2212)
{
// NOTE: No point of this.............
id = 2;
}
step = 0;
}
*/
}
}
