349 lines
10 KiB
C
Executable file
349 lines
10 KiB
C
Executable file
#include "stdafx.h"
|
|
|
|
int main()
|
|
{
|
|
//Init our robot
|
|
Robot* localRobot = new Robot();
|
|
|
|
sleep(1);//Wait for Hardware
|
|
|
|
//Init Displays
|
|
for(uint8 i = IO_DISPLAY_START; i < IO_DISPLAY_END; i++)
|
|
{
|
|
Display* newDisplay = new Display(i);
|
|
localRobot->AddModule(newDisplay);
|
|
newDisplay = NULL;
|
|
}
|
|
|
|
//Init Engines
|
|
for(uint8 i = IO_ENGINE_START; i < IO_ENGINE_END; i++)
|
|
{
|
|
Engine* newEngine = new Engine(i);
|
|
localRobot->AddModule(newEngine);
|
|
newEngine = NULL;
|
|
}
|
|
|
|
//Init Dribbler
|
|
for(uint8 i = IO_DRIBBLER_START; i < IO_DRIBBLER_END; i++)
|
|
{
|
|
Dribbler* newDribbler = new Dribbler(i);
|
|
localRobot->AddModule(newDribbler);
|
|
newDribbler = NULL;
|
|
}
|
|
|
|
//Init Kicker
|
|
for(uint8 i = IO_KICKER_START; i < IO_KICKER_END; i++)
|
|
{
|
|
Kicker* newKicker = new Kicker(i);
|
|
localRobot->AddModule(newKicker);
|
|
newKicker = NULL;
|
|
}
|
|
|
|
//Init Aktuator
|
|
for(uint8 i = IO_AKTUATOR_START; i < IO_AKTUATOR_END; i++)
|
|
{
|
|
Aktuator* newAktuator = new Aktuator(i);
|
|
localRobot->AddModule(newAktuator);
|
|
newAktuator = NULL;
|
|
}
|
|
|
|
//Init Wireless
|
|
for(uint8 i = IO_WIRELESS_START; i < IO_WIRELESS_END; i++)
|
|
{
|
|
Wireless* newWireless = new Wireless(i);
|
|
localRobot->AddModule(newWireless);
|
|
newWireless = NULL;
|
|
}
|
|
|
|
//Init Sensors
|
|
for(uint8 i = IO_SENSOR_START; i < IO_SENSOR_END; i++)
|
|
{
|
|
switch(i)
|
|
{
|
|
//Create correct type of sensor
|
|
case IO_SENSOR_IR_0_DEG:
|
|
case IO_SENSOR_IR_30_DEG:
|
|
case IO_SENSOR_IR_60_DEG:
|
|
case IO_SENSOR_IR_100_DEG:
|
|
case IO_SENSOR_IR_180_DEG:
|
|
case IO_SENSOR_IR_260_DEG:
|
|
case IO_SENSOR_IR_300_DEG:
|
|
case IO_SENSOR_IR_330_DEG:
|
|
{
|
|
IR_Sensor* newSensor = new IR_Sensor(i);
|
|
localRobot->AddModule(newSensor);
|
|
newSensor = NULL;
|
|
break;
|
|
}
|
|
case IO_SENSOR_DISTANCE_0_DEG:
|
|
case IO_SENSOR_DISTANCE_90_DEG:
|
|
case IO_SENSOR_DISTANCE_180_DEG:
|
|
case IO_SENSOR_DISTANCE_270_DEG:
|
|
{
|
|
Distance_Sensor* newSensor = new Distance_Sensor(i);
|
|
localRobot->AddModule(newSensor);
|
|
newSensor = NULL;
|
|
|
|
break;
|
|
}
|
|
case IO_SENSOR_MOUSE_LEFT:
|
|
case IO_SENSOR_MOUSE_RIGHT:
|
|
{
|
|
Mouse_Sensor* newSensor = new Mouse_Sensor(i);
|
|
localRobot->AddModule(newSensor);
|
|
newSensor = NULL;
|
|
|
|
break;
|
|
}
|
|
//Other cases
|
|
default:
|
|
{
|
|
Sensor* newSensor = new Sensor(i);
|
|
localRobot->AddModule(newSensor);
|
|
newSensor = NULL;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
//Init Leds
|
|
for(uint8 i = IO_LED_START; i < IO_LED_END; i++)
|
|
{
|
|
Led* newLed = new Led(i);
|
|
localRobot->AddModule(newLed);
|
|
newLed = NULL;
|
|
}
|
|
|
|
//Init Keyboards
|
|
for(uint8 i = IO_KEYBOARD_START; i < IO_KEYBOARD_END; i++)
|
|
{
|
|
Keyboard* newKeyboard = new Keyboard(i);
|
|
localRobot->AddModule(newKeyboard);
|
|
newKeyboard = NULL;
|
|
}
|
|
|
|
//Init Command Handler
|
|
for(uint8 i = IO_COMMAND_HANDLER_START; i < IO_COMMAND_HANDLER_END; i++)
|
|
{
|
|
Command_Handler* newCommandHandler = new Command_Handler(i);
|
|
localRobot->AddModule(newCommandHandler);
|
|
newCommandHandler = NULL;
|
|
}
|
|
|
|
//Init Obstacle Tracker
|
|
for(uint8 i = IO_OBSTACLE_TRACKER_START; i < IO_OBSTACLE_TRACKER_END; i++)
|
|
{
|
|
Obstacle_Tracker* newObstacleTracker = new Obstacle_Tracker(i);
|
|
localRobot->AddModule(newObstacleTracker);
|
|
newObstacleTracker = NULL;
|
|
}
|
|
|
|
//Init Position Tracker
|
|
for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
|
|
{
|
|
Position_Tracker* newPositionTracker = new Position_Tracker(i);
|
|
localRobot->AddModule(newPositionTracker);
|
|
newPositionTracker = NULL;
|
|
}
|
|
|
|
//Init Ball Tracker
|
|
for(uint8 i = IO_BALL_TRACKER_START; i < IO_BALL_TRACKER_END; i++)
|
|
{
|
|
Ball_Tracker* newBallTracker = new Ball_Tracker(i);
|
|
localRobot->AddModule(newBallTracker);
|
|
newBallTracker = NULL;
|
|
}
|
|
|
|
//Init Navigators
|
|
for(uint8 i = IO_NAVIGATOR_START; i < IO_NAVIGATOR_END; i++)
|
|
{
|
|
Navigator* newNavigator = new Navigator(i);
|
|
localRobot->AddModule(newNavigator);
|
|
newNavigator = NULL;
|
|
}
|
|
|
|
//Init Logics
|
|
for(uint8 i = IO_LOGIC_START; i < IO_LOGIC_END; i++)
|
|
{
|
|
Logic* newLogic = new Logic(i);
|
|
localRobot->AddModule(newLogic);
|
|
newLogic = NULL;
|
|
}
|
|
|
|
sleep(1);
|
|
|
|
//Debug code
|
|
Display* ourDisplay = localRobot->GetModule<Display>(IO_DISPLAY_MAIN);
|
|
ourDisplay->Clear();
|
|
ourDisplay->Print("Roboter fertig gestartet",1,1);
|
|
sleep(1);
|
|
|
|
|
|
IR_Sensor* ourSensor = NULL;
|
|
Distance_Sensor* distanceSensor = NULL;
|
|
uint16 value = 0;
|
|
int8 value2 = 0;
|
|
|
|
Command_Handler* ourCommandHandler = localRobot->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
|
|
uint32 i = 1;
|
|
Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
Position_Tracker* ourPositionTracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
|
|
Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN);
|
|
//ourDribbler->SetSpeed(1);
|
|
//ourDribbler->SetEnabled(true);
|
|
Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
|
|
Logic* ourLogic = localRobot->GetModule<Logic>(IO_LOGIC_MAIN);
|
|
Wireless* ourWireless = localRobot->GetModule<Wireless>(IO_WIRELESS_MAIN);
|
|
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200);
|
|
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
|
|
|
|
//sleep(2);
|
|
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200);
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);
|
|
|
|
//sleep(2);
|
|
|
|
|
|
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
|
|
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
|
|
|
|
/*(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(200);
|
|
(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetEnabled(true);*/
|
|
|
|
/*(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80);
|
|
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/
|
|
|
|
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(255);
|
|
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetEnabled(true);
|
|
|
|
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255);
|
|
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
|
|
|
|
sleep(2);
|
|
|
|
//Run
|
|
while(true)
|
|
{
|
|
i++;
|
|
|
|
/*if(!(i % 20))
|
|
{
|
|
ourDisplay->Clear();
|
|
msleep(10);
|
|
ourCommandHandler->PrintCommand();
|
|
}
|
|
|
|
localRobot->Update();
|
|
|
|
if(ourCommandHandler->displayDistanceSensors && !(i % 20))
|
|
{
|
|
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
|
|
value = distanceSensor->GetDistance();
|
|
ourDisplay->Print(value, 1, 4);
|
|
ourDisplay->Print(";");
|
|
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_90_DEG);
|
|
value = distanceSensor->GetDistance();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");
|
|
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_180_DEG);
|
|
value = distanceSensor->GetDistance();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");
|
|
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
|
|
value = distanceSensor->GetDistance();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");
|
|
}
|
|
|
|
if(ourCommandHandler->ticksPerCmOffset && !(i % 20))
|
|
{
|
|
ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1);
|
|
}
|
|
|
|
if(ourCommandHandler->displayPositionTracker && !(i % 20))
|
|
{
|
|
ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
|
|
ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
|
|
ourDisplay->Print(ourPositionTracker->GetOrientation(), 1, 4);
|
|
}
|
|
|
|
if(ourCommandHandler->displayPositionTracker && !(i % 20))
|
|
{
|
|
ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
|
|
ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
|
|
ourDisplay->Print(ourPositionTracker->GetOrientation() * 180.0f / PI, 1, 4);
|
|
}
|
|
|
|
if(ourCommandHandler->displayBallTracker && !(i % 20))
|
|
{
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 1, 2);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 6, 2);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 11, 2);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 16, 2);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 1, 3);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 6, 3);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 11, 3);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value, 16, 3);
|
|
ourDisplay->Print(";");
|
|
if(ourBallTracker->KnowsBallDirection())
|
|
{
|
|
ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
|
|
}
|
|
else
|
|
{
|
|
ourDisplay->Print("Ball not found", 1, 4);
|
|
}
|
|
|
|
if(ourBallTracker->HasBall()) {
|
|
ourDisplay->Print("Has Ball", 5, 4);
|
|
}
|
|
}
|
|
|
|
//if(!(i % 20))
|
|
//{
|
|
// ourDisplay->Print(ourWireless->Recieve(), 10, 1);
|
|
//}
|
|
|
|
if(ourBallTracker->KnowsBallDirection())
|
|
{
|
|
ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
|
|
}
|
|
else
|
|
{
|
|
ourNavigator->Stop();
|
|
}*/
|
|
}
|
|
|
|
//Cleanup
|
|
delete localRobot;
|
|
localRobot = NULL;
|
|
}
|