BOOL CdynControlDlg::OnInitDialog()
{
CDialogEx::OnInitDialog();
// Set the icon for this dialog. The framework does this automatically
// when the application's main window is not a dialog
SetIcon(m_hIcon, TRUE); // Set big icon
SetIcon(m_hIcon, FALSE); // Set small icon
// TODO: Add extra initialization here
// Read config file
FILE *fp = fopen("config.json", "r");
fseek(fp, 0, SEEK_END);
long len = ftell(fp);
fseek(fp, 0, SEEK_SET);
char *data = (char*)malloc(len + 1);
fread(data, 1, len, fp);
fclose(fp);
data[len] = '\0';
cJSON *root = cJSON_Parse(data);
m_configJSON = cJSON_GetObjectItem(root, "config");
m_servoJSON = cJSON_GetObjectItem(root, "servo");
m_actionJSON = cJSON_GetObjectItem(root, "action");
InitRobot();
m_num = cJSON_GetArraySize(m_servoJSON);
CRect degRect, speedRect;
GetDlgItem(IDC_STATIC)->GetWindowRect(degRect);
GetDlgItem(IDC_STATIC5)->GetWindowRect(speedRect);
int step = degRect.Height() / m_num;
for (int i = 0; i < m_num; i++) {
cJSON *element = cJSON_GetArrayItem(m_servoJSON, i);
CreatePositionBundle(element, degRect.left, degRect.top + step * i, degRect.Width() - 20, step);
CreateSpeedBundle(element, speedRect.left, speedRect.top + step * i, speedRect.Width() - 10, 30);
}
m_selectAll.SetCheck(BST_CHECKED);
cJSON *action = m_actionJSON->child;
while (action != NULL) {
m_predefined.AddString(charTotchar(action->string));
actions.push_back(action->string);
action = action->next;
}
return TRUE; // return TRUE unless you set the focus to a control
}
/** D E C L A R A T I O N S **************************************************/
void main(void) {
InitRobot();
for (;;) {
UINT32_VAL sensors;
sensors.Val = 0;
// char command;
//
// sensors = processSensor();
// if (sensors) {
// switch (sensors) {
// case 24:
// command = FORWARD;
// break;
// case 82:
// command = REVERSE;
// break;
// case 90:
// command = RIGHT;
// break;
// case 8:
// command = LEFT;
// break;
// case 28:
// command = HALT;
// break;
// default:
// command = HALT;
// break;
// }
// putch(command);
// moveRobot(command);
// }
sendIR(0xE718FF00);
sensors = processSensor();
printf("0x%X\r\n",sensors.word.HW);
Delay10KTCYx(200);
//__delay_ms(5);
// mLED_1_Toggle();
// Delay10KTCYx(200);
// Delay10KTCYx(200);
// Delay10KTCYx(200);
}
}
int main(int argc, char *argv[])
{
char host[100]="localhost";
char rob_name[20]="robsample";
float lPow,rPow;
int state=STOP, stoppedState=RUN, rob_id = 1;
int beaconToFollow=0;
printf( " Sample Robot\n Copyright (C) 2001-2013 Universidade de Aveiro\n" );
/* processing arguments */
while (argc > 2) /* every option has a value, thus argc must be 1, 3, 5, ... */
{
if (strcmp(argv[1], "-host") == 0)
{
strncpy(host, argv[2], 99);
host[99]='\0';
}
else if (strcmp(argv[1], "-robname") == 0)
{
strncpy(rob_name, argv[2], 19);
rob_name[19]='\0';
}
else if (strcmp(argv[1], "-pos") == 0)
{
if(sscanf(argv[2], "%d", &rob_id)!=1)
argc=0; /* error message will be printed */
}
else
{
break; /* the while */
}
argc -= 2;
argv += 2;
}
if (argc != 1)
{
fprintf(stderr, "Bad number of parameters\n"
"SYNOPSIS: mainRob [-host hostname] [-robname robotname] [-pos posnumber]\n");
return 1;
}
/* Connect Robot to simulator */
if(InitRobot(rob_name, rob_id, host)==-1)
{
printf( "%s Failed to connect\n", rob_name);
exit(1);
}
printf( "%s Connected\n", rob_name );
state=STOP;
while(1)
{
/* Reading next values from Sensors */
ReadSensors();
if(GetFinished()) /* Simulator has received Finish() or Robot Removed */
{
printf( "%s Exiting\n", rob_name );
exit(0);
}
if(state==STOP && GetStartButton()) state=stoppedState; /* Restart */
if(state!=STOP && GetStopButton()) {
stoppedState=state;
state=STOP; /* Interrupt */
}
switch (state) {
case RUN: /* Go */
if( GetVisitingLed() ) state = WAIT;
if(GetGroundSensor()==0) { /* Visit Target */
SetVisitingLed(1);
printf("%s visited target at %d\n", rob_name, GetTime());
}
else {
DetermineAction(0,&lPow,&rPow);
DriveMotors(lPow,rPow);
}
break;
case WAIT: /* Wait for others to visit target */
if(GetReturningLed()) state = RETURN;
DriveMotors(0.0,0.0);
break;
case RETURN: /* Return to home area */
if(GetGroundSensor()==1) { /* Finish */
Finish();
printf("%s found home at %d\n", rob_name, GetTime());
}
else {
DetermineAction(1,&lPow,&rPow);
DriveMotors(lPow,rPow);
}
break;
}
Say(rob_name);
//Request Sensors for next cycle
if(GetTime() % 2 == 0) {
RequestObstacleSensor(CENTER);
if(GetTime() % 8 == 0 || beaconToFollow == GetNumberOfBeacons())
RequestGroundSensor();
else
RequestBeaconSensor(beaconToFollow);
}
else {
RequestSensors(2, "IRSensor1", "IRSensor2");
}
}
return 1;
}
int main(int argc, char *argv[])
{
char cfg[MAX_FILE_NAME_SIZE]="config.json";
float lPow,rPow;
int state=STOP, stoppedState=RUN;
int beaconToFollow=0;
int ret = 0;
rob_cfg_t rob_cfg;
rob_state_t rob_state;
struct beaconMeasure beacon;
int totalBeacons = 0,curGroundSensor = -1;
double elapsed1 = 0.0, elapsed2 = 0.0, realTotal = 0.0;
struct timeval t1, t2, t3;
bool firstTimeStart = 1;
memset(&rob_state, 0, sizeof(rob_state_t));
/* processing arguments */
while (argc > 2)
{
if (strcmp(argv[1], "-cfg") == 0)
{
strncpy(cfg, argv[2], 99);
cfg[MAX_FILE_NAME_SIZE-1]='\0';
}
else
{
break; /* the while */
}
argc -= 2;
argv += 2;
}
cfg_parser_parse(cfg, &rob_cfg);
// int i;
// for(i = 0; i < rob_cfg.rob_viewer_size; i++)
// printf("Viewer: %s:%d\n", rob_cfg.rob_viewers[i].hostname, rob_cfg.rob_viewers[i].port);
InitJoystick(rob_cfg.joys_dev);
cfg_parser_connect_viewers(&rob_cfg);
/* Connect Robot to simulator */
if( InitRobot(rob_cfg.robo_name, rob_cfg.robo_pos, rob_cfg.hostname) == -1)
{
ret = 1;
printf( "%s Failed to connect\n", rob_cfg.robo_name);
}
else
{
totalBeacons = GetNumberOfBeacons();
printf( "Connected: %s, Total beacons: %d\n", rob_cfg.robo_name, totalBeacons);
state=STOP;
while(1)
{
/* Reading next values from Sensors */
ReadSensors();
if(GetFinished()) /* Simulator has received Finish() or Robot Removed */
{
printf( "Exiting: %s\n", rob_cfg.robo_name );
state = FINISHED;
gettimeofday(&t3, NULL);
elapsed2 = _get_elapsed_secs(&t2, &t3);
realTotal = _get_elapsed_secs(&t1, &t3);
printf("to beacon | to start | total | real total\n");
printf("& %.2f & %.2f & %.2f & %.2f \n", elapsed1, elapsed2, elapsed1 + elapsed2, realTotal);
break;
}
if(state==STOP && GetStartButton())
{
state=stoppedState; /* Restart */
if( firstTimeStart )
{
firstTimeStart = 0;
printf("Started counting elapsed time\n");
gettimeofday(&t1, NULL);
}
}
if(state!=STOP && GetStopButton()) {
stoppedState=state;
state=STOP; /* Interrupt */
}
curGroundSensor = GetGroundSensor();
switch (state)
{
case RUN: /* Go */
if( GetVisitingLed() )
{
gettimeofday(&t2, NULL);
elapsed1 = _get_elapsed_secs(&t1, &t2);
printf("Elapsed from origin to beacon: %f\n", elapsed1);
state = WAIT;
DriveMotors(0.0,0.0);
}
else
{
if( curGroundSensor == beaconToFollow )
{
beaconToFollow++;
SetVisitingLed(1);
printf("%s visited target at %d\n", rob_cfg.robo_name, GetTime());
}
else {
DetermineAction(beaconToFollow, &lPow, &rPow);
DriveMotors(lPow, rPow);
}
}
break;
case RETURN: /* Go */
if( curGroundSensor == totalBeacons )
{
printf("%s found home at %d\n", rob_cfg.robo_name, GetTime());
Finish();
}
else {
DetermineAction(beaconToFollow, &lPow, &rPow);
DriveMotors(lPow, rPow);
}
break;
case WAIT: /* Wait for others to visit target */
if(GetReturningLed())
{
SetVisitingLed(0);
state = RETURN;
gettimeofday(&t2, NULL);
}
DriveMotors(0.0,0.0);
break;
}
//Say(rob_cfg.robo_name);
rob_state.state = state;
if( (rob_state.leftAvail = IsObstacleReady(LEFT)) )
rob_state.left = GetObstacleSensor(LEFT);
if( (rob_state.rightAvail = IsObstacleReady(RIGHT)) )
rob_state.right = GetObstacleSensor(RIGHT);
if( (rob_state.centerAvail = IsObstacleReady(CENTER)) )
rob_state.center = GetObstacleSensor(CENTER);
if(IsGPSReady())
{
rob_state.x = GetX();
rob_state.y = GetY();
}
// if( IsGPSDirReady() )
// rob_state.dir = GetDir();
if( IsCompassReady() )
rob_state.dir = GetCompassSensor();
if( ( rob_state.beaconVis = IsBeaconReady(beaconToFollow) ) )
{
beacon = GetBeaconSensor(beaconToFollow);
if( ( rob_state.beaconVis = beacon.beaconVisible ) )
rob_state.beaconDir = beacon.beaconDir;
}
send_all_viewer_state_message(&rob_cfg, &rob_state);
RequestCompassSensor();
//Request Sensors for next cycle
if(GetTime() % 2 == 0) {
RequestObstacleSensor(CENTER);
if( (GetTime() % 8) == 0
|| beaconToFollow == totalBeacons )
RequestGroundSensor();
else
RequestBeaconSensor(beaconToFollow);
}
else {
RequestSensors(2, "IRSensor1", "IRSensor2");
}
}
send_all_viewer_state_message(&rob_cfg, &rob_state);
}
printf("Doing cleanup: %s\n", rob_cfg.robo_name);
CloseAndFreeJoystick();
cfg_parser_close(&rob_cfg);
return ret;
}
int main() {
int r;
#if GRAPHICS
struct Button *but,*button;
boolean quit=FALSE;
context = (struct Context *)malloc(sizeof(struct Context));
but= context->Buttons= CreateButton(RUN_BUTTON,"RUN/STOP",WINDOW_W-50,WINDOW_H-90);
but= but->Next=CreateButton(DISPLAY_RATE_UP_BUTTON,"+",WINDOW_W-40,WINDOW_H-60);
but= but->Next=CreateButton(DISPLAY_RATE_DOWN_BUTTON,"-",WINDOW_W-60,WINDOW_H-60);
but= but->Next=CreateButton(QUIT_BUTTON,"QUIT",WINDOW_W-50,WINDOW_H-30);
display_rate=1L;
OpenGraphics();
#endif
robots = (struct Robot **)malloc(NR*sizeof(struct Robot*));
for (r=0;r<NR;r++) {
robots[r] = (struct Robot *)malloc(sizeof(struct Robot));
}
InitFiles();
InitGlobal();
for (r=0;r<NR;r++) {
InitRobot(r,robots[r]);
}
#if GRAPHICS
while(quit == FALSE) {
button=PressButton(context);
switch(button->Value) {
case QUIT_BUTTON:
quit = TRUE;
break;
case DISPLAY_RATE_UP_BUTTON:
display_rate*=10L;
if (display_rate>100000L) display_rate=100000L;
display_rate-=display_rate%10L;
if (display_rate<1L) display_rate=1L;
DrawWindow(context);
break;
case DISPLAY_RATE_DOWN_BUTTON:
if (display_rate>9L) display_rate/=10L;
display_rate-=display_rate%10L;
if (display_rate<1L) display_rate=1L;
DrawWindow(context);
break;
case RUN_BUTTON:
#endif
do {
for (r=0;r<NR;r++) {
RunRobot(r,robots[r]);
EvalRobot(t,r,robots[r]);
}
t++;
if (t>TM) {
#if GRAPHICS
CloseGraphics();
#endif
return(1);
exit(1);
}
#if GRAPHICS
if (t%display_rate==0L) {
DrawWindow(context);
#if DISTY*(DISTY-2)
if (t%(250L*display_rate)==0L) {
DrawPlate(0,0,WINDOW_W-1,WINDOW_H-1,BLACK,PLATE_UP);
}
#endif
}
#endif
}
#if GRAPHICS
while (UnpressButton(context,button)==FALSE);
break;
#else
while (TRUE);
#endif
#if GRAPHICS
}
button->State = PLATE_UP;
}
CloseGraphics();
#endif
return(1);
}
