//This code measures how quickly a battery will drain (please start as fully charged)//
task main()
{
waitForStart();
nMotorEncoder(Right) = 0;
nMotorEncoder(Left) = 0;
wait1Msec(50);
ClearTimer(T2);
motor[Right] = speed;
motor[Left] = -speed;
AddToDatalog(speedDat, speed);
wait1Msec(8000);
//Get up to speed//
ClearTimer(T1);
startVolts = nAvgBatteryLevel;
while(time1(T1) < 10000){
}
//Starting encoder ticks per second//
encSpeed = ((nMotorEncoder(Right) / 10) + (abs(nMotorEncoder(Left)) / 10)) / 2;
AddToDatalog(encSpeedDat, encSpeed);
AddToDatalog(startVoltsDat, startVolts);
newSpeed = encSpeed;
//Run while at 80% of original speed or greater//
while(newSpeed >= (encSpeed * 80 / 100)){
ClearTimer(T1);
nMotorEncoder(Right) = 0;
nMotorEncoder(Left) = 0;
wait1Msec(50);
while(time1(T1) < 10000){
}
//Current encoder ticks per second//
newSpeed = ((nMotorEncoder(Right) / 10) + (abs(nMotorEncoder(Left)) / 10)) / 2;
AddToDatalog(newSpeedDat, newSpeed);
AddToDatalog(newVoltsDat, nAvgBatteryLevel);
}
time = time100(T2) / 10;
AddToDatalog(timeDat, time);
SaveNxtDatalog();
}
// Moves the arm to base height
void ArmBase()
{
ClearTimer(T1);
while(SensorValue[P1] + SensorValue[P2] < 2212 && time1[T1] < 3000)
{
Arm(127);
}
Trim();
}
// Moves the arm to wall height
void ArmWall()
{
ClearTimer(T1);
while(SensorValue[P1] + SensorValue[P2] < 2700 && time1[T1] < 4500)
{
Arm(127);
}
Trim();
}
task Foreground(){
int timeLeft;
while(true){
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long robotDist = nMotorEncoder[rtWheelMotor] + nMotorEncoder[ltWheelMotor];
int robotDir = (int)(nMotorEncoder[ltWheelMotor] - nMotorEncoder[rtWheelMotor]);
// Calculate the speed and direction commands
// int speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
// int dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir, path[pathIdx][DIRRL_IDX]);
// DebugInt("spd",speedCmd);
// DebugInt("dir",dirCmd);
#define FWDa 1
#define RT90a 2
int sm=FWD1;
int speedCmd=0;
int dirCmd=0;
switch sm {
case FWDa: //Drive Forward
speedCmd = ForwardDist(10, robotDist, 50);// CmdStopDist,,CmdSpeed
dirCmd=Direction(0, robotDir, 50);// CmdDir,,RateLimitValue
break;
default:
}
// Calculate the motor commands given the speed and direction commands.
// motor[ltWheelMotor]=speedCmd+dirCmd;
// motor[rtWheelMotor]=speedCmd-dirCmd;
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
task main()
{
//--------------------INIT Code---------------------------//
ForwardDistReset((tMotor)rtWheelMotor, (tMotor)ltWheelMotor);
DirectionReset();
//--------------------End INIT Code--------------------------//
StartTask(Foreground, 255);
while(true){
//-----------------Print the debug statements out------------------//
DebugPrint();
}// While
}// Main
int CenterGoal() {
ClearTimer(T4);
retractKnocker();
turnUltra(0, 90);
StartTask(raiseLift);
moveDistanceRamp(40, 20);
pause(1);
int position = detectPosition();
switch(position) {
case 1:
sound(1, 0.2);
CenterPosition1();
break;
case 2:
sound(2, 0.2);
CenterPosition2();
break;
case 3:
sound(3, 0.2);
CenterPosition3();
break;
default:
return -1;
}
deployClamp();
while(!lifted);
//int position = 2;
readAllSwitches();
//scoring commences
bool anything_pressed = false;
bool called_already = true;
translateRTHeading(100, 90);
while(!sideSwitch || !armSwitch) {
readAllSwitches();
if(tipSwitch) {
translating = false;
called_already = false;
move(-20);
}
else if(sideSwitch) {
translating = false;
called_already = false;
move(20);
}
else if(!called_already) {
translateRTHeading(100, 90);
called_already = true;
}
pause(0.2);
}
PlaySound(soundBeepBeep);
scoreBall();
move(0);
translating = false;
return position;
}
task main()
{
while (true)
{
if(joystick.joy1_TopHat == 0)
{
ClearTimer(T1);
int total = 0;
while(time1[T1] < 3000)
{
if(joystick.joy1_TopHat > -1)
{
total = total + joystick.joy1_TopHat;
}
}
if(total == 24)
{
ClearTimer(T1);
while(time1[T1] < 1000)
{
if(joy1Btn(3) == 1)
{
ClearTimer(T1);
while(time1[T1] < 1000)
{
if(joy1Btn(2) == 1)
{
ClearTimer(T1);
while(time1[T1] < 1000)
{
if(joy1Btn(9) == 1)
{
motor[leftMotor] = 50;
motor[rightMotor] = -50;
}
}
}
}
}
}
}
}
}
}
void turnPointLeft (float mRot, float mRotPerSec)
{
nxtDisplayCenteredTextLine(5, "TPL(%.2f,%.2f)",
mRot, mRotPerSec);
if (moveModeType != MMAllMoveTypes
&& moveModeType != MMTurnsOnly
&& moveModeType != MMPointTurnsOnly) {
return;
}
if (moveModeTiming == MMOneMoveAtATime) {
waitForTouch();
}
if (stepThroughMode == stepThroughModeOn) {
waitForTouch();
}
checkParameterRange(mRot, mRotPerSec);
int leftWheelInitial = nMotorEncoder[leftWheelMotor];
int rightWheelInitial = nMotorEncoder[rightWheelMotor];
int leftWheelTarget = nMotorEncoder[leftWheelMotor] - 360 * mRot;
int rightWheelTarget = nMotorEncoder[rightWheelMotor] + 360 * mRot;
ClearTimer(T1);
float motorPower = revolutionsPerSecondToMotorPower(mRotPerSec);
motor[leftWheelMotor] = -1 * motorPower;
motor[rightWheelMotor] = motorPower;
while ((nMotorEncoder[leftWheelMotor] > leftWheelTarget)
&& (nMotorEncoder[rightWheelMotor] < rightWheelTarget))
{
nxtDisplayCenteredTextLine(7, "%d", nMotorEncoder[leftWheelMotor]);
}
motor[leftWheelMotor] = 0;
motor[rightWheelMotor] = 0;
int leftWheelChange = nMotorEncoder[leftWheelMotor] - leftWheelInitial;
int rightWheelChange = nMotorEncoder[rightWheelMotor] - rightWheelInitial;
float revolutionsWheelsRotated =
((float) ( abs(leftWheelChange) > abs(rightWheelChange) ?
leftWheelChange : rightWheelChange ))
/ 360.0;
nxtDisplayCenteredTextLine(2, "TPL(%.2f,%.2f)",
mRot, mRotPerSec);
nxtDisplayCenteredTextLine(3, "%.2frev %.2fsec",
(float) revolutionsWheelsRotated, (float) time10(T1) / 100);
nxtDisplayCenteredTextLine(5, "");
nxtDisplayCenteredTextLine(7, "");
}
task main()
{
waitForStart();
ClearTimer(T1);
StartTask(firstMove);
while(true)
{
wait1Msec(1);
}
}
void raise()
{
nMotorPIDSpeedCtrl[lift] = mtrNoReg;
ClearTimer(T1);
while( time1[T1] < 1500 )
motor[lift] = -30;
motor[lift] = 0;
//nMotorPIDSpeedCtrl[lift] = mtrSpeedReg;
}
/*-------------------------------------------------------
WM_DESTROY message
---------------------------------------------------------*/
void OnDestroy(HWND hwnd)
{
ClearTimer();
KillTimer(hwnd, IDTIMER_TIMER);
if(g_hfontDialog) DeleteObject(g_hfontDialog);
PostQuitMessage(0);
}
void initRobot() {
servo[rampLatch] = 20;
servo[platLatch] = 255;
ClearTimer(T1); //Clear timer 1 for checking to see how long the robot waits before starting
initDrivetrain();//Initialize systems
initElevator();
StartTask(ElevatorControlTask); //begin elevator control task
StartTask(SignalLight);
setMode(IDLE);
}
// =======================================================================
// Function to move the robot by the gyro by time V2
// =======================================================================
void GyroTime_moveV2(int time, int power,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
relHeading =0;
Current_Angle=0; // reset current angle
long adj_power;
long adj_deg;
wait1Msec(200);
motor[LF_motor] = -power;
motor[RF_motor] = power;
motor[LB_motor] = -power;
motor[RB_motor] = power;
current_power = power;
ClearTimer(T1);
bool Done = false;
while(!Done)
{
/*nxtDisplayTextLine(1, "ad: %3d", adj_deg);
nxtDisplayTextLine(2, "R: %3d", (current_power + adj_power));
nxtDisplayTextLine(3, "L: %3d", (current_power - adj_power));
nxtDisplayTextLine(4, "ap: %3d", adj_power);*/
nxtDisplayBigTextLine(2, "S: %3d", SensorValue[SONAR]);
if(time1[T1] > time)
{
Done = true;
}
//----------------------------
/*if(ConstOrRel) Current_Speed=constHeading;
else Current_Speed=relHeading;
Current_Angle= Current_Angle + (float)(Current_Speed/GCPD);
wait1Msec(5);*/
//----------------------------
if(adjust == true)
{
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
/*adj_deg = (long) Current_Angle;
adj_power = adj_deg*GYRO_PROPORTION;*/
motor[LF_motor] = -(current_power - adj_power);
motor[RF_motor] = (current_power + adj_power);
motor[LB_motor] = -(current_power);
motor[RB_motor] = (current_power);
}
}
if(StopWhenDone==true)
{
motor[LF_motor] = 0;
motor[RF_motor] = 0;
motor[LB_motor] = 0;
motor[RB_motor] = 0;
}
}
//Atomatic controlling of the basket servo & sweeper
void BasketStateMachine()
{
static int state=CLOSED;
if(state==CLOSED &&
time1[T1]>=SERVOTIME &&
(SensorValue[touchSensor]==1 || joy2Btn(BASKETDOWNBUTTON)==true))
{
motor[sweeper]=-sweeperOn;
sweeperEnabled=false;
servo[basketServo]=basketServoDown;
ClearTimer(T1);
armEnabled=false;
state=OPENING;
}
else if(state==OPENING && time1[T1]>=SERVOTIME)
{
motor[sweeper]=off;
sweeperEnabled=true;
state=OPEN;
}
else if(state==OPEN && (ARMJOYSTICK>deadZone || joy2Btn(BASKETUPBUTTON)==true))
{
motor[sweeper]=sweeperOn;
sweeperEnabled=false;
servo[basketServo]=basketServoUp;
ClearTimer(T1);
state=CLOSING;
}
else if(state==CLOSING && time1[T1]>=SERVOTIME)
{
motor[sweeper]=off;
sweeperEnabled=true;
armEnabled=true;
ClearTimer(T1);
state=CLOSED;
}
}
void raiseLift()
{
ClearTimer(T1);
while(abs(nMotorEncoder[elevator]) < 8000 && time1[T1] <2500)
{
motor[elevator] = 100;
print(nMotorEncoder[elevator],4);
PlaySound(soundBlip);
}
motor[elevator] = 0;
}
task getaccel()
{
while(true)
{
ClearTimer(T1);
HTACreadAllAxes(Accel, X_axis, Y_axis, Z_axis);
X_axis = X_axis - offset_X;
wait10Msec(1);
X_axis_old = X_axis;
}
}
void armHeight(float percent) // uses a PID loop to drive the arm to a given height - used in autonomous
{
float goal = ( (percent/100) * (LEFT_ARM_UPPER_LIMIT - LEFT_ARM_LOWER_LIMIT) ) + LEFT_ARM_LOWER_LIMIT;
int output; // speed to send to the arm motots, set in the loop
InitPidGoal(armPid, goal); // initialize the arm PID with the goal
ClearTimer(T3); // clear the timer
while( (abs(armPid.error) > armErrorThreshold)
|| (abs(armPid.derivative) > armDerivativeThreshold) ) // until both the error and speed drop below acceptable thresholds...
{
if (time1(T3) > PID_UPDATE_TIME) // if enough time has passed since the last PID update...
{
output = UpdatePid(armPid, SensorValue(leftArmPot)); // ...update the motor speed with the arm PID...
setArmMotors(output); // ...and send that speed to the arm motors
ClearTimer(T3);
}
}
// don't set the motors back to 0 afterwards, we want them to hold position
wait1Msec(TIME_BETWEEN_AUTONOMOUS_ACTIONS); // lets things settle before moving to next action
}
void putLiftUpAuto(){
//put up lift to lower height
hogCPU();
motor[M_Lift]=100;
int safetime=1200; //1.2 seconds
ClearTimer(T4);
while(nMotorEncoder[M_Lift]<C_LIFTLOWTARGET-200 && time1[T4]<safetime){};
motor[M_Lift]=0;
if(time1[T4]>=safetime) StopAllTasks();
releaseCPU();
}
task robotHeading(){
ClearTimer(T1); // sets timer to 0
while(true)
{
int currentReading = HTGYROreadRot(HTGYRO) - initial; // gets the new sensor reading
heading += (currentReading) * (time1[T1] - lastTime) * .001; // modifies the header
if(heading>=360)
heading=heading-360;
if(heading<=0)
heading=heading+360;
lastTime = time1[T1]; // sets the last time for the next reading
if (time1[T1]>30000)
{ // this resets the timer after 30 seconds
ClearTimer(T1);
lastTime = 0;
}
radheading = heading/180*PI; // the heading expressed in radians
wait10Msec(1); // lets other tasks run
}
}
task failSafe()
{
ClearTimer(T1);
while(1)
{
if (time1 [T1] > 3500)
{
StopAllTasks();
}
}
}
// Moves robot forward for left and right distance
void Forward(int left, int right, int time = 20000)
{
// Formatting user input values
if(left < 0)
{
left = -left;
}
if(right < 0)
{
right = -right;
}
ClearTimer(T1);
// Hidden cumulative left and right values...
QLeft += left;
QRight += right;
// Calibrating left and right drives
// The larger it is, the earlier the drive stops
// The smaller it is, the later the drive stops
int leftConstant = 5;
int rightConstant = 5;
// Loop until both conditions have been satisfied
while(time1(T1) < time && (SensorValue[QuadLeft] < QLeft - leftConstant || SensorValue[QuadRight] < QRight - rightConstant))
{
// If left needs to move
if(SensorValue[QuadLeft] < QLeft - leftConstant)
{
// Driving left side
DriveLeft(40);
}
else
{
// Stopping left side
DriveLeft(0);
}
if(SensorValue[QuadRight] < QRight - rightConstant)
{
// Driving right side
DriveRight(40);
}
else
{
// Stopping right side
DriveLeft(0);
}
}
// Applying safety brake for safety measures
SafetyBrake();
}
float updateGyro()
{
if( time1[T2] > 4 )
{
int gVal = SensorValue(S3) - gyroOffset;
if( motor[FL] != 0 || motor[BR] != 0 )
gyroVal += (time1[T2] * gVal) / 1000.0;
//writeDebugStreamLine( "updateGyro gyroVal: %f gVal: %d Timer: %d", gyroVal, gVal, time1[T2] );
ClearTimer(T2); // reset gyro timer
}
return gyroVal;
}
void score()
{
motor[lift] = 75;
bool raising = false;
int timeValue = 7450;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (externalBattery == -1)
writeDebugStreamLine("%d", time1[T3]);
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
servo[output] = 244;
}
task gyroThread(){
//StartTask(graphicsThread);
ClearTimer(T4);
while(true){
int v=0;
v=readAnalogInput(HTPB,1);
// sonar1=readAnalogInput(HTPB,1);
/* sonar1=readAnalogInput(HTPB,1);
sonar2=readAnalogInput(HTPB,2);
sonar3=readAnalogInput(HTPB,3);
*/
int gyrovolts=v;
float dv=v-stationaryVoltage;
float dtheta=dv*valueDegreeSecond;
theta+=dtheta*time1[T4]*.001;
// updateTime=time1[T4];
ClearTimer(T4);
wait1Msec(2);
}
}
void ClearTimer(const std::string& name, bool disable)
{
boost::shared_lock<boost::shared_mutex> lock(mutex);
if (!hangdetectorthread)
return; //! Watchdog isn't running
ThreadNameToIdMap::iterator it = threadNameToId.find(name);
if (it == threadNameToId.end()) {
logOutput.Print("[Watchdog::ClearTimer] No thread found named \"%s\".", name.c_str());
return;
}
ClearTimer(disable, &(it->second));
}
static void stop_search()
{
ClearTimer(search_timer);
delete textout;
delete searchout;
free(stext);
stext = NULL;
search_mode = 0;
scale = savescale;
reflow_mode = savereflow;
SetEventHandler(PBMainFrame::main_handler/*main_handler*/);
}
task main() {
float r0 = getIRDir(sensorIR)-8, r1;
if(r0 > 0) rbtArcRight(-7); else rbtArcLeft(20);
rbtMoveFdDist(-10, 5000);
ClearTimer(T1); while(time1[T1] < 2000) {
r1 = getIRDir(sensorIR)-8; int acS[5]; HTIRS2readAllACStrength(sensorIR, acS[0], acS[1], acS[2], acS[3], acS[4]);
if(r0 > 0) {setLeftMotors(acS[4] > acS[3] ? -6 : -50); setRightMotors(acS[4] > acS[3] ? -50 : -8);}
else {setLeftMotors(acS[4] > acS[3] ? -6 : -90); setRightMotors(acS[4] > acS[3] ? -30 : 0);}
} setLeftMotors(0); setRightMotors(0); int cr = (r0 > 0 ? (r1 > 0 ? 1 : 2) : (r1 > 0 ? 3 : 4));
nxtDisplayBigTextLine(3, "%f", cr); for(;;);
}
task main()
{
StartTask(intakeStart);
while(SensorValue[bumperLeft]==0)
{
}
ClearTimer(T4);
moveSecondTierUp(127,450);
moveSecondTierDown(127,50);
intake = 1;
wait10Msec(50);
moveStraightDistance(127,200);
stopPid(0.6,0.3);
wait10Msec(10);
moveStraightDistance(30, 200);
stopPid(0.6,0.3);
wait10Msec(200);
intake = 0;
turnRight(100,250);
moveStraightDistance(100,100);
alignFoward(127);
wait10Msec(5);
stopDrive();
moveSharpRight(127,600);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveFirstTierUp(127,1800);
moveFirstTierDown(127,50);
crossRamp(127,300,0);
moveStraightTime(-127, 500);
if (time1[T4] < 10000)
{
moveSharpRight(127,50);
moveStraightDistance(127,250);
stopPid(0.6,0.3);
pushBridge(127,800);
moveSecondTierUp(100,200);
moveStraightDistance(-127,100);
turnRight(127,250);
alignFoward(127);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
moveStraightLight(127);
turnLeft(127,250);
moveStraightDistance(127,100);
stopPid(0.6,0.3);
stopLift();
}
StopTask(intakeStart);
}
void GameTimerManager::ClearAllTimers(Object* obj)
{
auto iter = mObjToHandlesMap.find(obj);
if (iter != mObjToHandlesMap.end())
{
for (auto& t : iter->second)
{
ClearTimer(t);
}
mObjToHandlesMap.erase(iter);
}
}
float updateGyro()
{
if( time1[T2] > 2 )
{
int gVal = SensorValue(S4) - gyroOffset;
if( motor[FrontR] != 0 || motor[BackL] != 0 )
gyroVal += (time1[T2] * gVal) / 1000.0;
ClearTimer(T2); // reset gyro timer
}
return gyroVal;
}
task main()
{
initializeRobot();
waitForStart();
ClearTimer(T2);
StartTask(firstMove);
StartTask(robotHeading);
StartTask(display);
while(true)
{
wait1Msec(1);
}
}
