//---------------------------------------------------------------------
// taskBalance
// This is the main balance task for the HTWay robot.
//
// Robot is assumed to start leaning on a wall. The first thing it
// does is take multiple samples of the gyro sensor to establish and
// initial gyro offset.
//
// After an initial gyro offset is established, the robot backs up
// against the wall until it falls forward, when it detects the
// forward fall, it start the balance loop.
//
// The main state variables are:
// gyroAngle This is the angle of the robot, it is the results of
// integrating on the gyro value.
// Units: degrees
// gyroSpeed The value from the Gyro Sensor after offset subtracted
// Units: degrees/second
// motorPos This is the motor position used for balancing.
// Note that this variable has two sources of input:
// Change in motor position based on the sum of
// MotorRotationCount of the two motors,
// and,
// forced movement based on user driving the robot.
// Units: degrees (sum of the two motors)
// motorSpeed This is the speed of the wheels of the robot based on the
// motor encoders.
// Units: degrees/second (sum of the two motors)
//
// From these state variables, the power to the motors is determined
// by this linear equation:
// power = KGYROSPEED * gyro +
// KGYROANGLE * gyroAngle +
// KPOS * motorPos +
// KSPEED * motorSpeed;
//
task taskBalance()
{
Follows(main);
float gyroSpeed, gyroAngle;
float motorSpeed;
int power, powerLeft, powerRight;
long tMotorPosOK;
long cLoop = 0;
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich beginne");
TextOut(0, LCD_LINE4, "mich nun");
TextOut(0, LCD_LINE4, "selbst zu");
TextOut(0, LCD_LINE6, "Stabilisieren!");
tMotorPosOK = CurrentTick();
// Reset the motors to make sure we start at a zero position
ResetRotationCount(LEFT_MOTOR);
ResetRotationCount(RIGHT_MOTOR);
while(true) {
CalcInterval(cLoop++);
GetGyroData(gyroSpeed, gyroAngle);
GetMotorData(motorSpeed, motorPos);
// Apply the drive control value to the motor position to get robot
// to move.
motorPos -= motorControlDrive * tInterval;
// This is the main balancing equation
power = (KGYROSPEED * gyroSpeed + // Deg/Sec from Gyro sensor
KGYROANGLE * gyroAngle) / ratioWheel + // Deg from integral of gyro
KPOS * motorPos + // From MotorRotaionCount of both motors
KDRIVE * motorControlDrive + // To improve start/stop performance
KSPEED * motorSpeed; // Motor speed in Deg/Sec
if (abs(power) < 100)
tMotorPosOK = CurrentTick();
SteerControl(power, powerLeft, powerRight);
// Apply the power values to the motors
OnFwd(LEFT_MOTOR, powerLeft);
OnFwd(RIGHT_MOTOR, powerRight);
// Check if robot has fallen by detecting that motorPos is being limited
// for an extended amount of time.
if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) {
break;
}
Wait(WAIT_TIME);
}
Off(MOTORS);
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich bin");
TextOut(0, LCD_LINE4, "hingefallen!");
TextOut(0, LCD_LINE5, "Neustart");
TextOut(0, LCD_LINE5, "erforderlich!");
}
void rotate()
{
OnRev(MOTOR_RIGHT, FORCE);
OnFwd(MOTOR_LEFT, FORCE);
}
task main()
{
EV3Init();
SetSensorTouch(S1);
SetSensorColour(S2);
SetSensorColour(S3);
SetSensorUS(S4);
int LBLACK;
int RBLACK;
int LGREEN;
int RGREEN;
int LYELLOW;
int RYELLOW;
int LBG;
int RBG;
int LGY;
int RGY;
//Read and Display Values
//Black
while(EnterButtonState() == NOTPRESSED);
LBLACK = SENSOR_2;
RBLACK = SENSOR_3;
TextNumOut(0, LCD_LINE1, "LBlck: ", LBLACK, 15);
TextNumOut(0, LCD_LINE2, "RBlck: ", RBLACK, 15);
Wait(500); //Threshold
//Green
while(EnterButtonState() == NOTPRESSED);
LGREEN = SENSOR_2;
RGREEN = SENSOR_3;
TextNumOut(0, LCD_LINE1, "LGrn: ", LGREEN, 15);
TextNumOut(0, LCD_LINE2, "RGrn: ", RGREEN, 15);
Wait(500);
//Yellow
while(EnterButtonState() == NOTPRESSED);
LYELLOW = SENSOR_2;
RYELLOW = SENSOR_3;
TextNumOut(0, LCD_LINE1, "LYlw: ", LYELLOW, 15);
TextNumOut(0, LCD_LINE2, "RYlw: ", RYELLOW, 15);
Wait(500);
//Calc. calibration values
while(EnterButtonState() == NOTPRESSED);
LBG = (LBLACK + LGREEN) / 2;
RBG = (RBLACK + RGREEN) / 2;
LGY = (LGREEN + LYELLOW) / 2;
RGY = (RGREEN + RYELLOW) / 2;
TextNumOut(0, LCD_LINE1, "LBG: ", LBG, 15);
TextNumOut(0, LCD_LINE2, "RBG: ", RBG, 15);
TextNumOut(0, LCD_LINE3, "LGY: ", LGY, 15);
TextNumOut(0, LCD_LINE4, "RGY: ", RGY, 15);
Wait(500);
//LBG = 10;
//RBG = 10;
//LGY = 100;
//RGY = 100;
//Line Follower
while(EnterButtonState() == NOTPRESSED);
while(SENSOR_2 < LGY || SENSOR_3 < RGY)
while(true)
{
TextNumOut(0, LCD_LINE1, "Tch : ", SENSOR_1, 15);
TextNumOut(0, LCD_LINE2, "LCol: ", SENSOR_2, 15);
TextNumOut(0, LCD_LINE3, "RCol: ", SENSOR_3, 15);
TextNumOut(0, LCD_LINE4, "US : ", SENSOR_4, 15);
if(SENSOR_2 > LGY)
{
OnFwd(OUT_A, -50);
OnFwd(OUT_D, 90);
}
else if(SENSOR_3 > RGY)
{
OnFwd(OUT_A, 90);
OnFwd(OUT_D, -50);
}
else if(SENSOR_2 < LBG)
{
OnFwd(OUT_A, -50);
OnFwd(OUT_D, 90);
}
else if (SENSOR_3 < RBG)
{
OnFwd(OUT_A, 90);
OnFwd(OUT_D, -50);
}
else OnFwd(OUT_AD, 80);
Wait(1);
}
//Vind het blikje
OnFwd(OUT_D, 100);
OnFwd(OUT_A, -100);
Wait(300);
OnFwd(OUT_A, 80);
OnFwd(OUT_C, -80);
while(SENSOR_4 > 25);
Wait(75);
OnFwd(OUT_AC, 100);
Wait(2000);
Off(OUT_AC);
EV3Exit();
return (EXIT_SUCCESS);
}
void backward()
{
OnFwd(MOTOR_LEFT, FORCE);
OnFwd(MOTOR_RIGHT, FORCE);
}
