task MapRoom()
{
float botLastXCoordinate = 0.0;
float botLastYCoordinate = 0.0;
float botCurrentXCoordinate = 0.0;
float botCurrentYCoordinate = 0.0;
int wallXCoordinate = 0;
int wallYCoordinate = 0;
int xCoordinateDisplayOffset = 50;
int yCoordinateDisplayOffset = 40;
startGyro();
resetGyro();
eraseDisplay();
while(true)
{
//Robot position
botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());
//Wall mapping
wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());
nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
ResetEncoderValue();
botLastXCoordinate = botCurrentXCoordinate;
botLastYCoordinate = botCurrentYCoordinate;
wait1Msec(20);
}
}
int main()
{
init_motors();
init_peripherals();
// find out how many cells are connected
u16 batteryCutoff;
while((batteryCutoff = getBatteryVoltage()) == 0);
if(batteryCutoff > 720) // voltage is higher than 6V -> 2cells
batteryCutoff = 720; // cutoff 6V
else
batteryCutoff = 360; // cutoff 3V
calibrate_sensors();
// enable interrupt driven gyro acquisation
startGyro();
u08 c = 0, maxThrust = (batteryCutoff>360)?70:100;
u08 cutBattery = 0;
while(1) {
if(Thrust > maxThrust)
Thrust = maxThrust;
calculate_balance(Thrust, Yaw, Pitch, Roll);
checkForInput();
// check battery
if(((c%100)==0)) {
u16 bat = getBatteryVoltage();
if(bat > 0 && bat <= batteryCutoff) {
if(maxThrust > 0)
maxThrust -= 5;
cutBattery = 1;
}
}
if(!cutBattery || !(c % 50))
PORTC ^= (1<<2);
c++;
//_delay_ms(100);
}
}
