task MapRoom()
{
float botLastXCoordinate = 0.0;
float botLastYCoordinate = 0.0;
float botCurrentXCoordinate = 0.0;
float botCurrentYCoordinate = 0.0;
int wallXCoordinate = 0;
int wallYCoordinate = 0;
int xCoordinateDisplayOffset = 50;
int yCoordinateDisplayOffset = 40;
startGyro();
resetGyro();
eraseDisplay();
while(true)
{
//Robot position
botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());
//Wall mapping
wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());
nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
ResetEncoderValue();
botLastXCoordinate = botCurrentXCoordinate;
botLastYCoordinate = botCurrentYCoordinate;
wait1Msec(20);
}
}
int searchSpot() {
//menelusuri garis hitam sampai menemukan "color"
int hue;
int colortemp;
int threshold = 65;
resetGyro(gyroSensor);
moveMotorTarget(leftMotor,360,100);
moveMotorTarget(rightMotor,360,100);
while(getMotorMoving(leftMotor)||getMotorMoving(rightMotor))
sleep(1);
while(1)
{
// sensor sees light:
if(getColorReflected(colorSensor) < threshold)
{
// counter-steer right:
motor[leftMotor] = 55;
motor[rightMotor] = 15;
}
// sensor sees dark:
else
{
// counter-steer left:
motor[leftMotor] = 15;
motor[rightMotor] = 55;
}
hue=getColorHue(colorSensor);
if(hue==99 || hue==253) {
colortemp=hue==99?green:red;
break;
}
}
return colortemp;
}
task main() {
initializeRobot();
bool programRunning = false;
while (true) {
wait1Msec(5);
if (programStarted() && !programRunning) { // I.E. program JUST started
wait1Msec(selection[DELAY_MENU]*1000); // Wait for selected delay
currentRoutine=selection[STARTPOS_MENU]; // Set routine
if (gyroEnabled()) StartTask(gyroTask); // Start gyro tracking
resetGyro(); // Set gyro to 0
resetDrive(); // Set encoders to 0
programRunning=true; // Set the running veriable so this doesn't execute again
}
if (programStarted()) {
if (DEBUG) nxtDisplayCenteredTextLine(4, "%i", currentRoutine);
//Constantly update arm movements
if (armState != 0) motor[BlockArm] = getTargetingPower(armState, nMotorEncoder[BlockArm], 0.04, 80);
if (currentInc < 100 && !done) { // make sure step never wraps
if (completed) {
//Start sequences
initRoutine(); currentInc++; // Increment step, make sure it doesn't ever wrap
}
runTargets();
}
} else {
processMenuInput();
}
}
}
void turnBack(){
resetGyro(gyroSensor);
moveMotorTarget(leftMotor,447,100);
moveMotorTarget(rightMotor,-447,100);
while ( getMotorMoving(leftMotor) || getMotorMoving(rightMotor) ) {
sleep(1);
}
}
void turn(int dir,int deg,int lmot,int rmot) {
//berbelok ke arah "dir", sebesar "deg" derajat
resetGyro(gyroSensor);
setMotorSpeed(leftMotor,lmot);
setMotorSpeed(rightMotor,rmot);
while(getMotorMoving(leftMotor) || getMotorMoving(rightMotor)) {
sleep(1);
if((getGyroDegrees(gyroSensor)<deg && dir==left)
||(getGyroDegrees(gyroSensor)>deg && dir==right))
break;
}
}
// turn right
bool Motoruino2::turnRight (int angle)
{
if (angle < -180 || angle > 180) return false;
// First Reset Gyro
resetGyro(true, false,false);
// Prepare the Params
motoruino2Comm.startNewComm(ADRRESS_SLAVE);
motoruino2Comm.addLongData( angle ); // Angle
// Send it and validate reply
return motoruino2Comm.sendDataReplyBool(TTDG);
}
int cekLine(int dir,int deg,int lmot,int rmot) {
//mengembalikan 1 jika menemukan garis
int threshold = 65;
resetGyro(gyroSensor);
setMotorSpeed(leftMotor,lmot);
setMotorSpeed(rightMotor,rmot);
while(getMotorMoving(leftMotor) || getMotorMoving(rightMotor)) {
sleep(1);
if(getColorReflected(colorSensor) < threshold)
return 1;
else if((getGyroDegrees(gyroSensor)<deg && dir==left)
||(getGyroDegrees(gyroSensor)>deg && dir==right))
break;
}
return 0;
}
//rotate the gyro clockwise is positive for this method
void rotate(int degrees) {
resetGyro(GYRO);
if(degrees < 0) {
while(getGyroDegrees(GYRO) < -degrees) {
setMotorSpeed(RIGHT_DRIVETRAIN_MOTOR, -100);
setMotorSpeed(LEFT_DRIVETRAIN_MOTOR, -100);
}
} else {
while(getGyroDegrees(GYRO) > -degrees) {
setMotorSpeed(LEFT_DRIVETRAIN_MOTOR, 100);
setMotorSpeed(RIGHT_DRIVETRAIN_MOTOR, 100);
}
}
setMotorSpeed(LEFT_DRIVETRAIN_MOTOR, 0);
setMotorSpeed(RIGHT_DRIVETRAIN_MOTOR, 0);
}
void turn( int deg, int power )
{
resetGyro();
if( deg < 0 )
power = -1 * power;
while( abs(gyroVal) < abs(deg) )
{
motor[right] = power;
motor[left] = -1 * power;
updateGyro();
}
motor[right] = 0;
motor[left] = 0;
}
void turn( int deg, int power, bool ramp)
{
resetGyro();
float rampMult = 1.0;
if( deg < 0 )
power = -1 * power;
while( abs(gyroVal) < abs(deg) )
{
if(ramp){rampMult = rampFunc(abs(gyroVal)/(float)abs(deg));}
motor[right] = power * rampMult;
motor[left] = -1 * power * rampMult;
updateGyro();
}
motor[right] = 0;
motor[left] = 0;
}
void turn( int deg, int power )
{
resetGyro();
if( deg > 0 )
power = -1 * power;
while( abs(gyroVal) < abs(deg) )
{
motor[FL] = power;
motor[BL] = power;
motor[FR] = power;
motor[BR] = power;
updateGyro();
}
motor[FL] = 0;
motor[BL] = 0;
motor[FR] = 0;
motor[BR] = 0;
}
void seekBounds() {
//Seek the left side of the line.
faceHeading(0);
setMotorSpeed(LeftMotor, -10);
setMotorSpeed(RightMotor, 10);
while(avgReflectedLight(5) < threshold || getGyroHeading(Gyro) > forwardsHeading*0.90) {
}
setMotorSpeed(LeftMotor, 0);
setMotorSpeed(RightMotor, 0);
resetGyro(Gyro);
//Seek the right side of the line.
setMotorSpeed(LeftMotor, 10);
setMotorSpeed(RightMotor, -10);
delay(500);
while(avgReflectedLight(5) < threshold) {
rightBound = getGyroHeading(Gyro);
}
forwardsHeading = rightBound/2;
}
void turn( int deg, int power )
{
resetGyro();
if( deg > 0 )
power = -1 * power;
ClearTimer(T1);
while( abs(gyroVal) < abs(deg)) // turn until the gyro is correct
{
if(time1[T1] > 3000) // timeout if stalled
break;
motor[FL] = power;
motor[BL] = power;
motor[FR] = power;
motor[BR] = power;
updateGyro();
}
motor[FL] = 0;
motor[BL] = 0;
motor[FR] = 0;
motor[BR] = 0;
}
/// rotate with counterclockwise (left) as positive
void rotate(int degrees, bool reset){
if(reset)
resetGyro(gyro);
float kP = 1;
if(degrees > 0){
// counterclockwise turn
while(getGyroHeading(gyro) < degrees){
setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
}
} else {
while(getGyroHeading(gyro) > degrees){
setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
}
}
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
void turn( float deg, float power )
{
resetGyro();
if( deg > 0 )
power = -1 * power;
ClearTimer(T1);
while( abs(gyroVal) < abs(deg)) // turn until the gyro is correct
{
if(time1[T1] > 3000) // timeout if stalled
Stop(1);
motor[FrontL] = -power;
motor[BackL] = -power;
motor[FrontR] = power;
motor[BackR] = power;
updateGyro();
writeDebugStreamLine("gyro is reading %f" , gyroVal);
}
motor[FrontL] = 0;
motor[BackL] = 0;
motor[FrontR] = 0;
motor[BackR] = 0;
}
void RobotBeta1::driveStrait(long maxTime) {
int slowDownProccessing = 0;
long cTime = 0;
resetGyro();
float angle = 0;
angle = gyro->GetAngle(); // get heading
GetWatchdog().SetEnabled(true);
while ((IsAutonomous()) && (cTime <= maxTime)) {
GetWatchdog().Feed();
float angle = gyro->GetAngle(); // get heading
if ((slowDownProccessing % 250) == 0) {
DBG("\n\t\tGyro Angle: %f\t\t\tDrive Adj: %f", gyro->GetAngle(), (angle * 0.03));
cout << "\n\t\tTime: "; cout << cTime; cout << "\t\t\tExit: "; cout << maxTime; cout << "\n";
}
slowDownProccessing++; cTime++;
robotDrive->Drive(-.5, (angle * 0.03));// turn to correct heading
Wait(0.004);
}
robotDrive->Drive(0.0, 0.0);
}
task main()
{
manualCallibColor();
resetGyro(Gyro);
seekBounds();
faceHeading(forwardsHeading);
while(true) {
if(isInsideLine()) {
setMotorSpeed(LeftMotor, 30);
setMotorSpeed(RightMotor, 30);
}
else {
seekBounds();
faceHeading(forwardsHeading);
eraseDisplay();
displayBigTextLine(0, "Head: %d", forwardsHeading);
}
}
}
/// drive straight using the gyro
void gyroDriveInches(float inches, int driveSpeed) {
inches = inches-2;
float kP = 2.5;
bool onTarget = false;
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetGyro(gyro);
while(!onTarget) {
if(getMotorEncoder(leftMotor) < inches/7.8) {
setMotorSpeed(leftMotor, driveSpeed + proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
}else {
onTarget = true;
}
if(getMotorEncoder(rightMotor) < inches/7.8) {
setMotorSpeed(rightMotor, driveSpeed - proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
}
}
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
task main() {
int leftMotorSpeed, rightMotorSpeed, wristMotorSpeed, armMotorSpeed, stickAValue, stickBValue, stickCValue, stickDValue;
resetMotorEncoder(armMotor);
resetMotorEncoder(clawMotor);
resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetMotorEncoder(wristMotor);
resetGyro(gyro);
startGyroCalibration(gyro, gyroCalibrateSamples2048);
eraseDisplay();
getGyroCalibrationFlag(gyro);
displayString(3, "calibrating gyro");
wait1Msec(500);
eraseDisplay();
startTask(displayMyMotorPositions);
while (true ){
/***************************************************************************************************************/
//Joystick Control:
/***************************************************************************************************************/
//Driving:
if(getJoystickValue(BtnEUp) > 0) { //Drive Straight Forward
driveStraightForward();
}
else if(getJoystickValue(BtnEDown) > 0) { //Drive Straight Backward
driveStraightBackward();
}
else {
driveForwardPressed = false;
driveBackwardPressed = false;
stickAValue = getJoystickValue(ChA);
if ((stickAValue <= 15) && (stickAValue >= -15) ) stickAValue = 0;
stickBValue = getJoystickValue(ChB);
if ((stickBValue <= 15) && (stickBValue >= -15) ) stickBValue = 0;
if (stickBValue >=0 ) stickBValue = (stickBValue / 10) * (stickBValue / 10) * 2;
else stickBValue = - (stickBValue / 10) * (stickBValue / 10) * 2;
leftMotorSpeed = stickAValue - stickBValue;
rightMotorSpeed = stickAValue + stickBValue;
if ( leftMotorSpeed > 100) leftMotorSpeed = 100;
if ( leftMotorSpeed < -100) leftMotorSpeed = -100;
if ( rightMotorSpeed > 100) rightMotorSpeed = 100;
if ( rightMotorSpeed < -100) rightMotorSpeed = -100;
if (leftMotorSpeed >=0 ) leftMotorSpeed = (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
else leftMotorSpeed = - (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
if (rightMotorSpeed >=0 ) rightMotorSpeed = (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
else rightMotorSpeed = - (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
setMotorSpeed(leftMotor, leftMotorSpeed);
setMotorSpeed(rightMotor, rightMotorSpeed);
}
/***************************************************************************************************************/
//WRIST MOTOR
stickDValue = getJoystickValue(ChD);
if ((stickDValue <= 15) && (stickDValue >= -15) ) stickDValue = 0;
wristMotorSpeed = stickDValue;
if (wristMotorSpeed >=0 ) wristMotorSpeed = (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
else wristMotorSpeed = - (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
globalWristPosition = getMotorEncoder(wristMotor);
if ((wristMotorSpeed > 0) && (globalWristPosition >= WRIST_UPPER_LIMIT)){
setMotorTarget(wristMotor, WRIST_UPPER_LIMIT, 70);
}
else if ((wristMotorSpeed < 0) && (globalWristPosition <= WRIST_LOWER_LIMIT)) {
setMotorTarget(wristMotor, WRIST_LOWER_LIMIT, 70);
}
else {
//slow things down if we are near the limit of wrist movement
if ( abs(globalWristPosition - WRIST_LOWER_LIMIT) < 10) {
wristMotorSpeed = wristMotorSpeed / 4;
}
if ( abs(globalWristPosition - WRIST_UPPER_LIMIT) < 10) {
wristMotorSpeed = wristMotorSpeed / 4;
}
setMotorSpeed(wristMotor, wristMotorSpeed );
}
/***************************************************************************************************************/
//ARM MOTOR
stickCValue = getJoystickValue(ChC);
if ((stickCValue <= 15) && (stickCValue >= -15) ) stickCValue = 0;
armMotorSpeed = stickCValue;
if (armMotorSpeed >=0 ) armMotorSpeed = (armMotorSpeed / 10) * (armMotorSpeed / 10);
else armMotorSpeed = - (armMotorSpeed / 10) * (armMotorSpeed / 10);
globalArmPosition = getMotorEncoder(armMotor);
if ((armMotorSpeed > 0) && (globalArmPosition >= ARM_UPPER_LIMIT)){
setMotorTarget(armMotor, ARM_UPPER_LIMIT, 70);
}
else if ((armMotorSpeed < 0) && (globalArmPosition <= ARM_LOWER_LIMIT)) {
setMotorTarget(armMotor, ARM_LOWER_LIMIT, 70);
}
else {
//slow things down if we are near the limit of arm movement
if ( abs(globalArmPosition - ARM_LOWER_LIMIT) < 10){
armMotorSpeed= armMotorSpeed/ 4;
}
if ( abs(globalArmPosition - ARM_UPPER_LIMIT) < 10){
armMotorSpeed= armMotorSpeed/ 4;
}
setMotorSpeed(armMotor, armMotorSpeed);
}
/***************************************************************************************************************/
//CLAW MOTOR
if(getJoystickValue(BtnLUp) > 0) { //CLOSE
setMotorSpeed(clawMotor, 70);
}
else if(getJoystickValue(BtnLDown) > 0) { //OPEN
globalClawPosition = getMotorEncoder(clawMotor);
if (globalClawPosition <= -87) {
setMotorTarget(clawMotor, -87, 70);
}
else {
setMotorSpeed(clawMotor, -70);
}
}
else {
globalClawPosition = getMotorEncoder(clawMotor);
setMotorTarget(clawMotor, globalClawPosition, 90);
}
/***************************************************************************************************************/
//Buttons to move our Arm for Us quickly to other positions
if(getJoystickValue(BtnFUp) > 0) { //UP
moveToAimingPosition();
}
if(getJoystickValue(BtnFDown) > 0) { //GRAB a Block
moveToNeutralPosition();
}
/***************************************************************************************************************/
//Block Stacking Positions
//Buttons to move our Arm for Us quickly to other positions
if(getJoystickValue(BtnRUp) > 0) { //Upper Stacking Position
moveToUpperStackingPosition();
}
if(getJoystickValue(BtnRDown) > 0) { //Lower Stacking Position
moveToLowerStackingPosition();
}
wait1Msec(50); // Give actions time to make progress and prevent over-control from taking inputs too fast
}
}
void setTarget(int modeIn, int maxTimeIn, long leftTargetIn, long rightTargetIn, unsigned int leftSpeedIn, unsigned int rightSpeedIn) {
// Note: If you're moving less than 1000 counts, set a lower speed than 100
mode = modeIn; // Define global mode
maxTime = maxTimeIn; //All modes accept maxTime
leftSpeed = leftSpeedIn; // These are unused in most instances but set them anyway
rightSpeed = rightSpeedIn;
completed = false; // Tell loop to reset whatever neccesary
if (!gyroEnabled() && mode==GYRO_DEGREES) mode=TURN_DEGREES; // If gyro is disabled switch to a raw turn
if (!gyroEnabled() && mode==ARC_GYRO_DEGREES) mode=ARC_DEGREES; //ditto
if (mode==RAW_MODE) {
leftTarget = leftTargetIn;
rightTarget = rightTargetIn;
} else if (mode==DRIVE_INCHES) {
leftTarget = leftTargetIn*INCH; //Set position times inch constant
rightTarget = rightTargetIn*INCH;
mode = RAW_MODE; // Inches have been converted; treat as raw
} else if (mode==TURN_DEGREES) {
rightTarget = leftTargetIn*DEGREES; // Also define approx motor counts incase fallback is needed
leftTarget = rightTarget*-1; // Negative power for other side
leftSpeed = AUTON_TURN_SPEED;
rightSpeed = AUTON_TURN_SPEED;
mode = RAW_MODE; // Degrees have been converted; treat as raw
} else if (mode==GYRO_DEGREES) {
setGyroPos(false);
resetGyro();gAngle=0;
leftTarget = leftTargetIn; // Assign degrees for gyro
} else if (mode==RAISE_ARM) {
armState = ARM_RAISED; // Set arm position
} else if (mode==LOWER_ARM) {
armState = -100;
} else if (mode==ELEVATE_ARM) {
armState = ARM_ELEVATED;
} else if (mode==OPEN_CLAMP) {
clampState = false; // Set clamp position
} else if (mode==CLOSE_CLAMP) {
clampState = true;
} else if (mode==DRIVE_UP_RAMP) {
int sel = selection[RAMP_MENU];
if (sel==0 && gyroEnabled()) { //Drive until level
setGyroPos(true); // Run gyro-setting routine
mode=INTERNAL_RAMP;
} else { //Time-based ramp
leftTarget=-10000;
rightTarget=-10000; //Unattainably far position for max power
leftSpeed=100;
rightSpeed=100;
mode=RAW_MODE;
switch(sel) {
case 1: maxTime=1600; break; //Drive to hump
case 2: maxTime=1300; break; //Drive to front
case 3: maxTime=2000; break; //Drive to back
}
}
} else if (mode==ARC_DEGREES) {
mode = RAW_MODE;
leftSpeed=100;
rightSpeed=100;
if (leftTargetIn != 0) {
leftTarget=leftTargetIn*DEGREES_ARC;
rightTarget=0;
} else { // Right
leftTarget=0;
rightTarget=-rightTargetIn*DEGREES_ARC; //note the negative
}
} else if (mode==ARC_GYRO_DEGREES) {
setGyroPos(false);
resetGyro();
if (leftTargetIn != 0) {
leftSpeed=1;
rightSpeed=0;
leftTarget = leftTargetIn;
} else { // Right
leftSpeed=0;
rightSpeed=1;
leftTarget = rightTargetIn;
}
} else if (mode==SCOUT_RAMP) {
// setTarget(SCOUT_RAMP, SCOUT_LEFT or SCOUT_RIGHT, distance behind ramp edge, 0,0,0)
bool dir = (maxTime==SCOUT_RIGHT);
int distTo1 = 2400;
int distTo2 = 5000;
int sel = selection[SONAR_MENU];
if (fallbackMode==true) sel=3;
if (sel==0) {
findClearRamp((dir ? distanceRight : distanceLeft), leftTargetIn+distTo2);
} else if (sel==1) {
findLine(leftTargetIn+distTo2, 1);
} else if (sel==2) {
findLine(leftTargetIn+distTo2, 2);
} else if (sel==3) { //Fallback mode
mode=RAW_MODE;
maxTime=3000;
leftTarget = -3200;
rightTarget = -3200;
leftSpeed = 100;
rightSpeed = 100;
}
}
time1[T2]=0; // Start clock
return;
}
//Predefined construct
void pre_auton()
{
bStopTasksBetweenModes = true;
resetGyro();
}
void Drive::resetDrive()
{
resetDriveEncoders();
resetGyro();
}
void pre_auton()
{
setUp();
resetGyro();
resetDrive();
}
