task usercontrol()
{
// User control code here, inside the loop
while (true)
{
motor[DFR]=vexRT[Ch2];
motor[DBR]=vexRT[Ch2];
motor[DFL]=vexRT[Ch2];
motor[DBL]=vexRT[Ch2];
motor[middle]=vexRT[Ch1];
if(abs(vexRT[Ch4]) > MIN_JOYSTICK_ACTIVATION_THRESHOLD)
{
turnRight(127);
}
else
{
turnRight(0);
}
if(abs(vexRT[Ch3]) > tol)//set straight
straight = vexRT[Ch3];
else
straight = 0;
if(abs(vexRT[Ch1]) > tol) //set ptturn
ptturn = vexRT[Ch1];
else
ptturn = 0;
if(abs(vexRT[Ch4]) > tol) //set side
side = vexRT[Ch4];
else
side = 0;
if((abs(-ptturn + straight)<=127) && (abs(ptturn + straight)<=127))
{
driveRight(-ptturn/2 + straight*2/3);
driveLeft(ptturn/2 + straight*2/3);
motor[holo]= side;
}
else if(abs(-ptturn + straight) > 127)//(pttrn + straight)
{
driveRight(100);
driveLeft(-100);
motor[holo]= side;
}
else if(abs(-ptturn + straight) > 127)
{
driveRight(-100);
driveLeft(100);
motor[holo]= side;
}
}
}
void *displayStart()
{
slotA2D();
slotI2C();
setDisplayToOutput();
i2cFileDesc = initI2cBus(I2CDRV_LINUX_BUS2, I2C_DEVICE_ADDRESS);
writeI2cReg(i2cFileDesc, REG_DIRA, 0x00);
writeI2cReg(i2cFileDesc, REG_DIRB, 0x00);
//Clear Display
writeI2cReg(i2cFileDesc, REG_BOT, 0x00);
writeI2cReg(i2cFileDesc, REG_TOP, 0x00);
deActivateRight();
deActivateLeft();
while(1) {
//Do stuff here
driveLeft();
driveRight();
}
deActivateRight();
deActivateLeft();
close(i2cFileDesc);
}
void drive_straight(float dist, int speed)
{
while((nMotorEncoder[DBR] < 360 * dist / (4 * PI) + error) || (nMotorEncoder[DBR] < 360 * dist / (4 * PI) - error))
{
driveRight(speed);
driveLeft(speed);
}
}
// ----------------------------------------------------------------------------
// drive using light ambient sensors for directional command
// ------------------------------------------------------------[ driveByLight ]
void WRTbotDrive::driveByLight( wrt_motor_t &motor )
{
// -------------------------------------------------------------------
if( ( 0 == (motor.left.control & wrt_motorControl_power_bit) ) ||
( 0 == (motor.right.control & wrt_motorControl_power_bit) ) )
{
WRTbotDrive::stop();
return;
}
// --- read ambient light sensor
// -------------------------------------------------------------------
sensor.light.data = analogRead( wrt_pin_ambientLight_sensor );
Serial.println(" ");
Serial.print( sensor.light.data ); Serial.print( " , " );
// --- convert to ambient light units
// -------------------------------------------------------------------
sensor.light.data *= sensor.light.factor;
Serial.print( sensor.light.data ); Serial.println( " , " );
useNominal = true;
// -------------------------------------------------------------------
if( (sensor.light.data > sensor.light.limitMin) && // minimum limit
(sensor.light.data < sensor.light.limitLowerBound) ) // highest lower bound
{
Serial.print( "driveLeft" ); Serial.println( " " );
driveLeft( motor );
}
else if( (sensor.light.data > sensor.light.limitUpperBound) && // lowest upper bound
(sensor.light.data < sensor.light.limitMax) ) // maximum limit
{
Serial.print( "driveRight" ); Serial.println( " " );
driveRight( motor );
}
else
{
Serial.print( "driveForward" ); Serial.println( " now " );
driveForward( motor );
}
} // done driveByLight()
task main(){
motor[motorB] = 0;
motor[motorC] = 0;
drivefor(1,75);
driveRight(75);
driveLeft(75);
drivefor(1,-75);
encodeTurnB(360, 25);
encodeTurnC(360, 25);
simpleTurn("B",3);
simpleTurn("C",3);
}
void Motor_run()
{
if(MPI_check_available(MOTOR_DRIVER_PID) == true)
{
MPI_get_message(MOTOR_DRIVER_PID, &incomingPID, &cmdSize, incomingMsg);
if(cmdSize == sizeof(int))
{
OS_memcpy(&incomingCmd, incomingMsg, cmdSize);
switch(incomingCmd)
{
case C_DRIVE_FORWARD:
UART_send("drive forward\r\n", CONSOLE_BASE);
driveForward();
break;
case C_DRIVE_REVERSE:
UART_send("drive reverse\r\n", CONSOLE_BASE);
driveReverse();
break;
case C_DRIVE_LEFT:
UART_send("drive left\r\n", CONSOLE_BASE);
driveLeft();
break;
case C_DRIVE_RIGHT:
UART_send("drive right\r\n", CONSOLE_BASE);
driveRight();
break;
case C_DRIVE_HALT:
UART_send("drive halt\r\n", CONSOLE_BASE);
driveHalt();
break;
}
}
}
}
void turn(int a)
{
driveRight(-a);
driveLeft(a);
}
// drive the robot
// -------------------------------------------------------------------[ drive ]
void WRTbotDrive::drive( const uint16_t driveThisWay, wrt_motor_t &motor )
{
// --- motor ON by default
// -----------------------
motor.left.control |= wrt_motorControl_power_ON;
motor.right.control |= wrt_motorControl_power_ON;
// --- no drive by light
// --------------------------
this->driveByLightFactor = 0;
switch( driveThisWay )
{ default:
// ------------------------------------------------------------[ stop ]
case wrt_stop:
{
motor.left.control &= wrt_motorControl_power_OFF;
motor.right.control &= wrt_motorControl_power_OFF;
// ---
motor.left.speed = 0;
motor.right.speed = 0;
// ---
drive( motor );
} break;
// ---------------------------------------------------------[ forward ]
case wrt_forward:
{
this->useNominal = true;
// ---
driveForward( motor );
} break;
// ---------------------------------------------------------[ forward ]
case wrt_forward_slow:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_min;
motor.left.speed = wrt_code_motor_speed_min;
// ---
driveForward( motor );
} break;
// ----------------------------------------------------[ fast forward ]
case wrt_forward_fast:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_max;
motor.left.speed = wrt_code_motor_speed_max;
// ---
driveForward( motor );
} break;
// ---------------------------------------------------------[ reverse ]
case wrt_reverse:
{
this->useNominal = true;
// ---
driveReverse( motor );
} break;
// ----------------------------------------------------[ fast reverse ]
case wrt_reverse_fast:
{
this->useNominal = false;
motor.right.speed = wrt_code_motor_speed_max;
motor.left.speed = wrt_code_motor_speed_max;
// ---
driveReverse( motor );
} break;
// -------------------------------------------------------[ left turn ]
case wrt_drive_left:
{
driveLeft( motor );
} break;
// --------------------------------------------------[ fast left turn ]
case wrt_turn_left_fast:
{
turnLeftFast( motor );
} break;
// --------------------------------------------------[ slow left turn ]
case wrt_turn_left:
{
this->useNominal = true;
turnLeft( motor );
} break;
// ------------------------------------------------------[ right turn ]
case wrt_drive_right:
{
driveRight( motor );
} break;
// -------------------------------------------------[ fast right turn ]
case wrt_turn_right_fast:
{
turnRightFast( motor );
} break;
// -------------------------------------------------[ slow right turn ]
case wrt_turn_right:
{
this->useNominal = true;
turnRight( motor );
} break;
// -----------------------------------------[ differential right turn ]
case wrt_turnRightDiff:
case wrt_turnRightDiff_fast:
{
turnRightDiferential( driveThisWay, motor );
} break;
// -----------------------------------------[ differential left turn ]
case wrt_turnLeftDiff:
case wrt_turnLeftDiff_fast:
{
turnLeftDiferential( driveThisWay, motor );
} break;
}
} // done drive()
