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); } }