311 lines
8.4 KiB
C
Executable file
311 lines
8.4 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 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();
|
|
|
|
|
|
IR_Sensor* ourSensor = NULL;
|
|
Distance_Sensor* distanceSensor = NULL;
|
|
uint16 value = 0;
|
|
int8 value2 = 0;
|
|
|
|
Keyboard* ourKeyboard = localRobot->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
|
|
uint32 i = 1;
|
|
Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
Position_Tracker* ourPosition_Tracker = 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);
|
|
|
|
float rotation = 0;
|
|
float speed = 200;
|
|
|
|
//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
|
|
//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
|
|
|
|
|
|
//Run
|
|
while(true)
|
|
{
|
|
ourDisplay->Print(i++,1,1);
|
|
|
|
msleep(100);
|
|
|
|
if(!(i % 1))
|
|
{
|
|
ourDisplay->Clear();
|
|
//ourDisplay->Print(ourPosition_Tracker->GetPositionX(),1,2);
|
|
//ourDisplay->Print(ourPosition_Tracker->GetPositionY(),1,3);
|
|
//ourDisplay->Print(ourPosition_Tracker->GetOrientation() * 180.0f / PI,1,4);
|
|
}
|
|
|
|
if(!(i % 50))
|
|
{
|
|
ourAktuator->Kick();
|
|
}
|
|
|
|
|
|
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
|
|
value = distanceSensor->GetDistance();
|
|
ourDisplay->Print(value, 1, 2);
|
|
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, 1, 3);
|
|
ourDisplay->Print(";");
|
|
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_270_DEG);
|
|
value = distanceSensor->GetDistance();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");
|
|
|
|
uint8 someInput = ourKeyboard->GetInput();
|
|
//ourDisplay->Print("Ready to accept...", 1, 2);
|
|
switch(someInput)
|
|
{
|
|
case 0:
|
|
ourNavigator->Stop();
|
|
return 0;
|
|
break;
|
|
case 1:
|
|
ourNavigator->Drive(225, rotation, speed, rotation);
|
|
break;
|
|
case 2:
|
|
ourNavigator->Drive(180, rotation, speed, rotation);
|
|
break;
|
|
case 3:
|
|
ourNavigator->Drive(135, rotation, speed, rotation);
|
|
break;
|
|
case 4:
|
|
ourNavigator->Drive(270, rotation, speed, rotation);
|
|
break;
|
|
case 5:
|
|
ourNavigator->Drive(0, rotation, 0, rotation);
|
|
break;
|
|
case 6:
|
|
ourNavigator->Drive(90, rotation, speed, rotation);
|
|
break;
|
|
case 7:
|
|
ourNavigator->Drive(315, rotation, speed, rotation);
|
|
break;
|
|
case 8:
|
|
ourNavigator->Drive(0, rotation, speed, rotation);
|
|
break;
|
|
case 9:
|
|
ourNavigator->Drive(45, rotation, speed, rotation);
|
|
break;
|
|
case 10:
|
|
ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
|
|
break;
|
|
case 12:
|
|
ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
|
|
break;
|
|
}
|
|
|
|
//ourDisplay->Clear();
|
|
/*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);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value);
|
|
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);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");
|
|
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
|
|
value = ourSensor->GetIRIntensity();
|
|
ourDisplay->Print(value);
|
|
ourDisplay->Print(";");*/
|
|
|
|
localRobot->Update();
|
|
|
|
//ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
|
|
|
|
//ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
|
|
}
|
|
|
|
//Cleanup
|
|
delete localRobot;
|
|
localRobot = NULL;
|
|
}
|