int main(void)
{
/* Initialize the SAM system */
SystemInit();
/* Initialize pins */
Pin_Configuration();
/* Initialize PWM generator */
InitPWMController_MCLK();
/* Initialize timers */
Configure_Timers();
InitMotors();
/* Configre UART */
configure_uart();
sendString("###ON \n", 7);
sendString("###Initializing\n", 16);
/* Disable watchdog */
WDT->WDT_MR |= WDT_MR_WDDIS;
//selfTest();
// Set variables
initializeMotors = 0;
flag12 = 0;
// ----- TASK_1
#if defined(TASK_1)
while (1)
{
if(getNewSpeed())
{
sendString("###New Speed\n", 14);
newSpeed = 0;
ControlledDrive(percentage_ST,percentage_DR);
flag12 = 0;
}
}
// ----- TASK_2
#elif defined(TASK_2)
while (1)
{
}
// ----- TASK_3
#elif defined(TASK_3)
startStop_Camera = 0;
while (1)
{
if(initializeMotors)
{
sendString("###Initializing\n", 16);
InitMotors();
initializeMotors = 0;
}
if(startStop_Camera)
{ // Stop motors and ignore lidar values
WriteMotors(0,0);
}
else
{ // Start and go based on lidar values
if(getNewSpeed())
{
sendString("###New Speed\n", 14);
newSpeed = 0;
ControlledDrive(percentage_ST,percentage_DR);
flag12 = 0;
}
//ForwardDrive();
}
}
// ----- TASK_4
#elif defined(TASK_4)
while (1)
{
}
#endif
}
// This is the actual task that is run
static portTASK_FUNCTION( vMotorControlTask, pvParameters )
{
// Get the parameters
param = (motorControlStruct *) pvParameters;
// Get the I2C task pointer
i2cData = param->i2cData;
// Get the Navigation task pointer
webData = param->webData;
// Get the LCD task pointer
lcdData = param->lcdData;
prevSpeedVal = MOTOR_FORWARD_SPEED;
curSpeedVal = MOTOR_FORWARD_SPEED;
currentOp = NONE;
lastOp = NONE;
rightEncoderCount = 0;
leftEncoderCount = 0;
currentTime = 0;
speedDiff = 0;
forward = 0;
reverse = 0;
right = 0;
left = 0;
msg[0] = 'f';
msg[1] = ' ';
msg[4] = 'b';
msg[5] = ' ';
msg[8] = 'r';
msg[9] = ' ';
msg[13] = 'l';
msg[14] = ' ';
msg[18] = ' ';
msg[19] = 0;
// Like all good tasks, this should never exit
for(;;)
{
// Wait for a message from the I2C (Encoder data) or from the Navigation Task (motor command)
if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
VT_HANDLE_FATAL_ERROR(Q_RECV_ERROR);
}
//sendi2cMotorMsg(i2cData, '!', 1, portMAX_DELAY);
switch(getMsgType(&msgBuffer)){
case motorTimerMsgType:
{
currentTime++;
if(currentOp != NONE)
{
if(currentTime >= targetVal)
{
sendi2cMotorMsg(i2cData,MOTOR_STOP_SPEED + RIGHT_MOTOR_OFFSET,MOTOR_STOP_SPEED, portMAX_DELAY);
currentOp = NONE;
currentTime = 0;
}
}
if(currentTime%10 == 0){
switch(lastOp){
case FORWARD:
{
forward = forward + getCentimeters(rightEncoderCount, leftEncoderCount);
//updateMoveForwardMsg(navData,getCentimeters(rightEncoderCount, leftEncoderCount));
break;
}
case REVERSE:
{
reverse = reverse + getCentimeters(rightEncoderCount, leftEncoderCount);
//updateMoveBackwardMsg(navData,getCentimeters(rightEncoderCount, leftEncoderCount));
break;
}
case RIGHT:
{
right = right + getDegrees(rightEncoderCount, leftEncoderCount);
//updateRotateClockwiseMsg(navData,getDegrees(rightEncoderCount, leftEncoderCount));
break;
}
case LEFT:
{
left = left + getDegrees(rightEncoderCount, leftEncoderCount); //This is exactly right... Because difference will be negative... Think this out later.
//updateRotateCounterclockwiseMsg(navData,getDegrees(rightEncoderCount, leftEncoderCount));
break;
}
default:
{
break;
}
}
rightEncoderCount = 0;
leftEncoderCount = 0;
#ifdef SEND_COUNTS_TO_LCD
msg[2] = (forward / 10) % 10 + 48;
msg[3] = forward % 10 + 48;
msg[6] = (reverse / 10) % 10 + 48;
msg[7] = reverse % 10 + 48;
msg[10] = (right / 100) % 10 + 48;
msg[11] = (right / 10) % 10 + 48;
msg[12] = right % 10 + 48;
msg[15] = (left / 100) % 10 + 48;
msg[16] = (left / 10) % 10 + 48;
msg[17] = left % 10 + 48;
//SendLCDPrintMsg(lcdData, 20, msg);
#endif
}
break;
}
case setDirForwardMsgType:
{
currentOp = FORWARD;
lastOp = FORWARD;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)*TIMER_COUNTS_PER_CENTIMETER;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case setDirReverseMsgType:
{
currentOp = REVERSE;
lastOp = REVERSE;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)*TIMER_COUNTS_PER_CENTIMETER;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case setMotorSpeedMsgType:
{
speedDiff = curSpeedVal - getNewSpeed(&msgBuffer);
prevSpeedVal = curSpeedVal;
curSpeedVal = curSpeedVal + speedDiff;
break;
}
case turnRightMsgType:
{
currentOp = RIGHT;
lastOp = RIGHT;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)/DEGREES_PER_TIMER_COUNT;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case turnLeftMsgType:
{
currentOp = LEFT;
lastOp = LEFT;
currentTime = 0;
leftEncoderCount = 0;
rightEncoderCount = 0;
//targetVal = getTargetVal(&msgBuffer)/DEGREES_PER_TIMER_COUNT;
sendi2cMotorMsg(i2cData, curSpeedVal + RIGHT_MOTOR_OFFSET, curSpeedVal, portMAX_DELAY);
break;
}
case motorStopMsgType:
{
currentOp = NONE;
//prevSpeedVal = MOTOR_STOP_SPEED; // This is undoubtedly incorrect so look at this Matt!!
//curSpeedVal = MOTOR_STOP_SPEED; // This is undoubtedly incorrect so look at this Matt!!
sendi2cMotorMsg(i2cData, MOTOR_STOP_SPEED + RIGHT_MOTOR_OFFSET, MOTOR_STOP_SPEED, portMAX_DELAY);
currentTime = 0;
break;
}
case encoderDataMsgType:
{
rightEncoderCount += getRightCount(&msgBuffer);
leftEncoderCount += getLeftCount(&msgBuffer);
// char msg[12];
// msg[0] = 'L';
// msg[1] = ':';
// msg[2] = ' ';
// msg[3] = (getLeftCount(&msgBuffer) / 10) % 10 + 48;
// msg[4] = getLeftCount(&msgBuffer) % 10 + 48;
// msg[5] = ' ';
// msg[6] = 'R';
// msg[7] = ':';
// msg[8] = ' ';
// msg[9] = (getRightCount(&msgBuffer) / 10) % 10 + 48;
// msg[10] = getRightCount(&msgBuffer) % 10 + 48;
// msg[11] = 0;
// SendLCDPrintMsg(lcdData, 11, msg);
break;
}
default:
{
VT_HANDLE_FATAL_ERROR(UNKNOWN_MOTOR_CONTROL_MSG_TYPE);
break;
}
}
}
}