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;
}
void turnLeft(){
moveMotorTarget(leftMotor,-220,200);
moveMotorTarget(rightMotor,220,200);
while ( getMotorMoving(leftMotor) || getMotorMoving(rightMotor) ) {
sleep(1);
}
}
void moveForward(int degree){
moveMotorTarget(leftMotor,degree,50);
moveMotorTarget(rightMotor,degree,50);
while ( getMotorMoving(leftMotor) || getMotorMoving(rightMotor) ) {
sleep(1);
}
}
void stepAhead(int degree) {
//maju degree derajat putaran roda
moveMotorTarget(leftMotor,degree,50);
moveMotorTarget(rightMotor,degree,50);
while(getMotorMoving(leftMotor)||getMotorMoving(rightMotor)) {
sleep(1);
}
}
void turnBack(){
resetGyro(gyroSensor);
moveMotorTarget(leftMotor,447,100);
moveMotorTarget(rightMotor,-447,100);
while ( getMotorMoving(leftMotor) || getMotorMoving(rightMotor) ) {
sleep(1);
}
}
void turn(int direction,int degree) {
//berbelok ke arah "direction", sebesar "degree"
int rightpower=direction==right?-71:71;
moveMotorTarget(leftMotor,degree,-rightpower);
moveMotorTarget(rightMotor,-degree,rightpower);
while(getMotorMoving(leftMotor) || getMotorMoving(rightMotor)) {
sleep(1);
}
}
// void leftparking()// did all this assuming motorA is left, motorB is right, colorSensorA is left, colorSensorB is right
task sonarFunction()
{
while(true)
{
moveMotorTarget(motorC, 90, 50);
waitUntilMotorStop(motorC);
moveMotorTarget(motorC, -90, -50);
waitUntilMotorStop(motorC);
motor[motorC] = 0;
}
}
//Controls all movement tasks
void motor_execute(motor_control* motor_tasks,byte* head_motor,byte* start_motor,system_state* current_state)
{
if ((*start_motor) <= 0) //Completed task
{
if ((((getMotorTargetCompleted(motor1)) && (getMotorTargetCompleted(motor2))) || (time1[T1] > FAILOVER_TIME)) || ((*start_motor) == -1))
{
//Task completed
resetMotorEncoder(motor1); //Stop motor
motor[motor1] = 0;
moveMotorTarget(motor1, 0, 0, 0);
resetMotorEncoder(motor2);
motor[motor2] = 0;
moveMotorTarget(motor2, 0, 0, 0);
byte i;
*current_state = motor_tasks[0].next_state;
for (i=0; i< (*head_motor) ;++i) //Delete first task
{
motor_tasks[i].motor1_power = motor_tasks[i+1].motor1_power;
motor_tasks[i].motor1_encoder = motor_tasks[i+1].motor1_encoder;
motor_tasks[i].motor2_power = motor_tasks[i+1].motor2_power;
motor_tasks[i].motor2_encoder = motor_tasks[i+1].motor2_encoder;
motor_tasks[i].next_state = motor_tasks[i+1].next_state;
}
if ((--(*head_motor)) == 0)
--(*head_motor);
else
*start_motor = 1;
}
#if DEBUG
writeDebugStreamLine("Task ended");
#endif
}
else if ((*start_motor) == 1) //Start moving
{
#if DEBUG
writeDebugStreamLine("Staring task");
#endif
clearTimer(T1); //Failover timer
resetMotorEncoder(motor1);
moveMotorTarget(motor1, motor_tasks[0].motor1_encoder, motor_tasks[0].motor1_power, 0);
resetMotorEncoder(motor2);
moveMotorTarget(motor2, motor_tasks[0].motor2_encoder, motor_tasks[0].motor2_power, 0);
*start_motor = 0;
}
}
task rightparking()
{
if(SensorValue[sonarSensor] <= 75)
{
motor[motorL] = 75; // lego group x*y x=diamter, y=width
motor[motorR] = 75;
}
else
{
motor[motorL] = 90;
motor[motorR] = 0;
wait1Msec(100);
moveMotorTarget(motorL, 2122, 75);
moveMotorTarget(motorR, 2122, 75);
waitUntilMotorStop(motorR);
motor[motorL] = 0;
motor[motorR] = 0;
}
}
int checkLine(int dir) {
int a,degmax=0;
int incdegree=dir==right?10:-10;
int rightpower=dir==right?-50:50;
int rightdegree=dir==right?-10:10;
while((degmax<2300&&dir==right) || (degmax>-2300&&dir==left)) {
a=check_color();
switch(a) {
case 0:
moveMotorTarget(leftMotor,-rightdegree,-rightpower);
moveMotorTarget(rightMotor,rightdegree,rightpower);
while(getMotorMoving(leftMotor) && getMotorMoving(rightMotor))
sleep(1);
degmax+=incdegree;
break;
case 1:
motor[leftMotor]=0;
motor[rightMotor]=0;
sleep(100);
return 1;
}
}
}
// Wrapper to move the motor, provides additional debugging feedback
void moveMotorAxis(tMotor axis, float degrees, long speed){
writeDebugStreamLine("moveMotorAxis: motor: %d, degrees: %f, speed: %d", axis, degrees, speed);
#ifndef DISABLE_MOTORS
// SPEED PARAM
long motorSpeed = speed;
long degreesi = round(degrees);
if (degreesi < 0)
{
degreesi = abs(degreesi);
motorSpeed = -speed;
}
moveMotorTarget(axis, degreesi, motorSpeed);
#endif
return;
}
void armClose()
{
moveMotorTarget(armMotor, 100, 75); //Set motor to run 100 counts at power level 75.
}