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;
}
}
}
}
int main(void) {
bt_init(BAUD_RATE);
Sonar_init(CONTINUOUS);
Servo_init(MANUAL, SCAN_AND_LOCK);
motorDriverInit();
while(1){
bt_getStr( tab ); // Get string from buffer
if(strlen( tab )){ // If isn't empty...
//bt_sendStr( tab ); // ...send it back.
if ( strcmp(tab, "w") == 0 ) {
driveForward(speed);
}
else if ( strcmp(tab, "s") == 0) {
driveReverse(speed);
}
else if ( strcmp(tab, " ") == 0) {
stop();
}
else if ( strcmp(tab, "a") == 0) {
driveReverseLeftTrack(40);
driveForwardRightTrack(40);
}
else if ( strcmp(tab, "d") == 0) {
driveForwardLeftTrack(40);
driveReverseRightTrack(40);
}
else if ( strcmp(tab, "ServoScanAndLock") == 0) {
ServoChangeMode(SWEEP);
ServoChangeSweepMode(SCAN_AND_LOCK);
}
else if ( strcmp(tab, "ServoScanAndGo") == 0) {
ServoChangeMode(SWEEP);
ServoChangeSweepMode(SCAN_AND_GO);
}
else if ( strcmp(tab, "ServoCenter") == 0) {
ServoChangeMode(MANUAL);
ServoMoveByDegree(0);
}
else if ( strncmp(tab, "SonarStartMeas",14) == 0) {
int new_deg = ParseIntNumber(tab,14,3);
SonarStartMeas(new_deg);
}
else if ( strncmp(tab, "SonarGetDistance",16) == 0) {
char buffor[12];
int new_deg = ParseIntNumber(tab,16,3);
sprintf(buffor, "%04d,%04hu\n",new_deg,SonarGetDistance(new_deg));
bt_sendStr(buffor);
}
else if ( strcmp(tab, "e") == 0) {
if (speed<=90){
speed+=10;
}
}
else if ( strcmp(tab, "q") == 0) {
if (speed>=10){
speed-=10;
}
}
else if (strncmp(tab, "speed",5) == 0){
int new_speed = ParseIntNumber(tab,5,3);
if (new_speed >= 0 && new_speed <= 100){
speed = new_speed;
}
}
else if ( strncmp(tab, "SonarLockRange",14) == 0) {
int new_range = ParseIntNumber(tab,14,3);
ServoChangeLockRange(new_range);
}
}
}
};
// 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()
