void speedUp(U8 motorPowerAdjust)
{
S32 adjustedThrottle = (acc.throttleAmount + (floor((acc.throttleAmount - 1) / 100.0) * motorPowerAdjust));
setThrottleAmount(adjustedThrottle);
controlMotors(acc.throttleAmount, BRAKE_OFF);
}
void maintainDistance()
{
acc.maintainCounter ++;
if(acc.maintainCounter > MAINTAIN_TIMES_BEFORE_ACTION)
{
/*Within accepted boundries - Maybe calibrate (very litte) to better match leading vehicle*/
if(acc.throttleAmount <= MINIMUM_POWER_FOR_DRIVE)
{
if(acc.firstDistanceMeasurement > acc.secondDistanceMeasurement) //Mindre afstand
{
setThrottleAmount(acc.throttleAmount - 1);
controlMotors(acc.throttleAmount, BRAKE_OFF);
}
else if(acc.firstDistanceMeasurement < acc.secondDistanceMeasurement)
{
setThrottleAmount(acc.throttleAmount + 1);
controlMotors(acc.throttleAmount, BRAKE_OFF);
}
}
acc.maintainCounter = 0;
}
}
void examineID(msg_pointer mp){
printf("ID is %d\n", mp->ID);
if (mp->ID == START_ID)
startMotors();
if (mp->ID == STOP_ID)
stopMotors();
if (mp-> ID == CONTROL_ID)
controlMotors(mp);
if (mp-> ID == SPECIAL_COMMAND_ID)
specialMotorCommand(mp);
return;
}
void ACC(float actualTimeBetweenVehicles, S32 distanceToVehicle, float leadVehicleVelocity)
{
if(actualTimeBetweenVehicles > MAXIMUM_TIME_BETWEEN_VEHICLES) //No vehicle to follow or too far away. (Zone A)
{
controlMotors(INITIAL_MOTOR_POWER, BRAKE_OFF);
/*Drive accordingly to normal Cruise Control*/
}
else if(actualTimeBetweenVehicles < MINIMUM_TIME_BETWEEN_VEHICLES) //Vehicle too close. (Zone E)
{
//brake();
slowDown(MOTOR_POWER_ADJUST);
}
else
{
if(leadVehicleVelocity > vehicle.vFront)
{//Vehicle in front drives faster than set speed.
if((DESIRED_TIME_BETWEEN_VEHICLES - MAXIMUM_DEVIATION) > actualTimeBetweenVehicles &&
MINIMUM_TIME_BETWEEN_VEHICLES < actualTimeBetweenVehicles)
{//Vehicle in front is too close to cruise but not close enought to brake. (Zone D)
slowDown(MOTOR_POWER_ADJUST);
}
else
{ //(Zone C)
maintainDistance();
}
}
else //Vehicle in front drives slower than set speed.
{
if((DESIRED_TIME_BETWEEN_VEHICLES + MAXIMUM_DEVIATION) >= actualTimeBetweenVehicles &&
(DESIRED_TIME_BETWEEN_VEHICLES - MAXIMUM_DEVIATION) <= actualTimeBetweenVehicles)
{//Within tolerance. (Zone C)
maintainDistance();
}
else
{//(Zone B)
speedUp(MOTOR_POWER_ADJUST);
}
}
acc.firstDistanceMeasurement = acc.secondDistanceMeasurement;
}
}
/*
* to get the message and redirect to propper function
*/
uint8_t examineID(msg_pointer mp){
switch(mp->ID){
case START_ID:{
moto_startMotors();
break;
}
case STOP_ID:{
moto_stopMotors();
break;
}
case CONTROL_ID:{
controlMotors(mp);
break;
}
case SPECIAL_COMMAND_ID:{
specialMotorCommand(mp);
break;
}
}
return 0;
}
