task main(){ enableCompetitionMode(); //This makes it so the Vex Competition machine can take control repeat(forever){ if (getCompetitionAutonomous() == true) {roboControl();} //Competition robo Controls if (getCompetitionDriverControl() == true) {manualControl();} //Competition driver Controls } }
int main(void) { // init IOs signal(SIGINT,sigfunc); if (pfio_init() < 0) exit(-1); // init PWM if (wiringPiSetup () == -1) exit (1); pinMode (1, PWM_OUTPUT); laserPower = POW_MED; setZero(JUMP); printf("plotd: daemon ready\n"); manualControl(); pfio_write_output(0x00); pfio_deinit(); return 0; }
/*APRIMORAR E TESTAR*/ void menu(void){ int b1, b2, digital = 0; int menu = 0; while(!digital){ digital = readJSButton(); b1 = readButtons('1'); b2 = readButtons('2'); if(b1||b2){ menu++; menu %= 2; } if(menu == 0){ TM_HD44780_Clear(); TM_HD44780_Puts(0, 0, "MODO AUTONOMO"); TM_HD44780_PutCustom(15, 0, 0); TM_HD44780_Puts(0, 1, "MODO MANUAL"); Delayms(50); }else if(menu == 1){ TM_HD44780_Clear(); TM_HD44780_Puts(0, 0, "MODO AUTONOMO"); TM_HD44780_Puts(0, 1, "MODO MANUAL"); TM_HD44780_PutCustom(15, 1, 0); Delayms(50); } TM_HD44780_Clear(); } if(menu == 0){ modoPerfuracao(); //setDesenhaQuadrado(); }else if(menu == 1){ manualControl(); } }
void Pilot::handleControl() { _Cmd* command = NULL; rapidjson::Document* doc = NULL; int uid = -1; pthread_mutex_lock(&_RecvLock); if(!_RecvList.empty()){ command = _RecvList.begin()->element; _RecvList.erase(_RecvList.begin()); } pthread_mutex_unlock(&_RecvLock); if(command) { bool ret = true; uid = command->id; doc = new rapidjson::Document; do{ doc->Parse<0>(command->cmd.c_str()); if(doc->HasParseError()||doc->IsObject() == false||doc->HasMember("name") == false|| doc->HasMember("args") == false ) { ret = false; break; } if(!strcmp((*doc)["name"].GetString(),"ControlState")) { const rapidjson::Value& pArgs = (*doc)["args"]; if(pArgs.IsArray()&&pArgs[rapidjson::SizeType(0)].IsInt()) { int s = pArgs[rapidjson::SizeType(0)].GetInt(); _PilotState = s; resetControl(); if(_PilotState == PILOT_AUTOCONTROL&&_autoScript.length() != 0) { _autoController->close(); _autoController->init(_autoScript.c_str(),this); } } ret = false; } }while(0); if(ret == false) { delete doc; doc = NULL; } delete command; } if(_hardware&&_hardware->getStatus()->isAllUpdated() == true) { _hardware->getStatus()->copyStatus(_status); _hardware->getStatus()->reset(); } if(_sendStatusTimer.elapsed(1000)) { sendStatus(); } switch (_PilotState) { case PILOT_MANUAL: if(doc) manualControl(doc,uid); break; case PILOT_HALFMANUAL: if(doc) halfManualControl(doc,uid); break; case PILOT_AUTOCONTROL: autoControl(); break; default: _PilotState = PILOT_MANUAL; break; } if(doc != NULL) delete doc; }
void Stage::loop() { //Check for manual commands manualControl(); //Test limit switches to prevent driving stage past limits if(!digitalRead(Z_ULIMIT_SWITCH) && (getDistanceToGo(Z_STEPPER) > 0)){ Move(Z_STEPPER,0); } if(!digitalRead(Z_LLIMIT_SWITCH) && (getDistanceToGo(Z_STEPPER) < 0)){ Move(Z_STEPPER,0);; } //Called on every loop to enable non-blocking control of steppers if((millis()-_z_last_step)>_z_interval){ if((_z_target-_z_pos)>0){ z_1_motor->onestep(FORWARD, DOUBLE); z_2_motor->onestep(FORWARD, DOUBLE); _z_last_step = millis(); _z_pos++; } else if((_z_target-_z_pos)<0){ z_1_motor->onestep(BACKWARD, DOUBLE); z_2_motor->onestep(BACKWARD, DOUBLE); _z_last_step = millis(); _z_pos--; } } if((millis()-_xy_last_step)>_xy_interval){ if((_x_target-_x_pos)>0 && (_y_target-_y_pos)>0){ //Move up and right xy_a_motor->onestep(FORWARD, INTERLEAVE); _xy_last_step = millis(); _x_pos++; _y_pos++; } else if((_x_target-_x_pos)>0 && (_y_target-_y_pos)<0){ //Move down and right xy_b_motor->onestep(FORWARD, INTERLEAVE); _xy_last_step = millis(); _x_pos++; _y_pos--; } else if((_x_target-_x_pos)<0 && (_y_target-_y_pos)>0){ //Move up and left xy_b_motor->onestep(BACKWARD, INTERLEAVE); _xy_last_step = millis(); _x_pos--; _y_pos++; } else if((_x_target-_x_pos)<0 && (_y_target-_y_pos)<0){ //Move down and left xy_a_motor->onestep(BACKWARD, INTERLEAVE); _xy_last_step = millis(); _x_pos--; _y_pos--; } else if((_x_target-_x_pos)==0 && (_y_target-_y_pos)>0){ //Move up xy_a_motor->onestep(FORWARD, INTERLEAVE); xy_b_motor->onestep(BACKWARD, INTERLEAVE); _xy_last_step = millis(); _y_pos++; } else if((_x_target-_x_pos)==0 && (_y_target-_y_pos)<0){ //Move down xy_a_motor->onestep(BACKWARD, INTERLEAVE); xy_b_motor->onestep(FORWARD, INTERLEAVE); _xy_last_step = millis(); _y_pos--; } else if((_x_target-_x_pos)>0 && (_y_target-_y_pos)==0){ //Move right xy_a_motor->onestep(FORWARD, INTERLEAVE); xy_b_motor->onestep(FORWARD, INTERLEAVE); _xy_last_step = millis(); _x_pos++; } else if((_x_target-_x_pos)<0 && (_y_target-_y_pos)==0){ //Move left xy_a_motor->onestep(BACKWARD, INTERLEAVE); xy_b_motor->onestep(BACKWARD, INTERLEAVE); _xy_last_step = millis(); _x_pos--; } } }
int main(int argc, char **argv) { int ADCvalue01 = 0; int StepSpeed = 100; //Stepping speed (smaler value = faster) int i = 0; int KeyHit = 0; int KeyCode[5]; int SingleKey=0; char TextLine[300]; char FileName[100] = ""; char FileNameOld[100] = ""; char FullFileName[300]; char a; char IsDigit[]="-1234567890"; int ReadState = 0; int MenueLevel = 0; int FileSelected = 0; int FileStartRow = 0; int stopPlot = 0; long xMin = 1000000, xMax = -1000000; long yMin = 1000000, yMax = -1000000; long coordinateCount = 0; long coordinatePlot = 0; long currentPlotX = 0, currentPlotY = 0, currentPlotDown = 1; long xNow = 0, yNow = 0, zNow = 0; char *pEnd; FILE *PlotFile; double Scale = 1.0; double OldScale = 1.0; double PicWidth = 0.0; double PicHeight = 0.0; long MoveLength = 100; long OldMoveLength = 200; long PlotStartTime = 0; int MoveFinished = 0; long stepPause = 5000; long stepPauseNormal = 5000; long stepPauseMaterial = 20000; long zHub = 200; int plotterMode = 1; char FileInfo[3]; long FileSize; long LongTemp; long DataOffset; long HeaderSize; long PictureWidth; long PictureHeight; int IntTemp; int ColorDepth; long CompressionType; long PictureSize; long XPixelPerMeter; long YPixelPerMeter; long ColorNumber; long ColorUsed; int PixelRed, PixelGreen, PixelBlue; int PixOK = 0; long StepsPerPixelX = 50, StepsPerPixelY = 50; long JetOffset1 = 40, JetOffset2 = 40; //Mapping between wiringPi and pin order of my I/O board. //This was necessary, because I used a wrong description of the GPIOs //You have to change the order according to your board! //@@@THESE ARE ALL OUTPUS!! strcpy(FileName, "noFiLE"); if (wiringPiSetup () == -1){ printf("Could not run wiringPiSetup!"); exit(1); } getmaxyx(stdscr, MaxRows, MaxCols); MessageY = MaxRows-3; clrscr(1, MaxRows); PrintRow('-', MessageY - 1); initscr(); noecho(); cbreak(); start_color(); while(!quit) { clearScreen(); char * choices[] = {"Plot File","Manual Control","Servo Control","Opto Sensor Control","X&Y Control","initalize","Exit"}; char *description[] = { "You select a file to plot and will plot the file on the plotter", "Will give you full control over the plotter for debuging or demenstration", "Lets you enter and test different duration of pulses sent to the servo for fine tuning and debugging", "Will read in the togleing of the opto sensor for debuging", "Will let you have control how much to move in x and y by ticks more precisly", "Test initalization procedure", "Will exit the prgram"}; int result = menus("Hello and Welcome to my creation please sleect a mode", choices,description,7,20); switch (result) { case 1: ; /* FILE * fp; char * line = NULL; size_t len = 0; ssize_t read; fp = fopen ("sampleGcode.txt","r");*/ //Plot file char dir[1024]; getcwd(dir,sizeof(dir)); MessageText("3 seconds until plotting starts !!!!!!!!!!!!!!!!!", 1, 2, 0); delay(3000); if((PlotFile=chooseFile(dir))==NULL){ sprintf(TextLine, "Can't open file '%s'!\n", FullFileName); strcpy(FileName, "NoFiLE"); ErrorText(TextLine); } else{ currentPlotX = 0; currentPlotY = 0; PlotStartTime = time(0); PrintMenue_03(FullFileName, coordinateCount, 0, 0, 0, PlotStartTime,Scale); coordinatePlot = 0; stopPlot = 0; if(currentPlotDown == 1){ penMove(1); currentPlotDown = 0; } while(!(feof(PlotFile)) && stopPlot == 0){ fread(&a, 1, 1, PlotFile); if(a == '>'){ ReadState = 0; } if(ReadState == 0 || ReadState == 1){ TextLine[i] = a; TextLine[i+1] = '\0'; i++; } if(ReadState == 2 || ReadState == 3){ if(strchr(IsDigit, a) != NULL){ TextLine[i] = a; TextLine[i+1] = '\0'; i++; } } if(ReadState == 2){ if(strchr(IsDigit, a) == NULL){ if(strlen(TextLine) > 0){ xNow = (strtol(TextLine, &pEnd, 10) - xMin) * 0.33333 * Scale; ReadState = 3; i=0; TextLine[0]='\0'; } } } if(ReadState == 3){ if(strchr(IsDigit, a) == NULL){ if(strlen(TextLine) > 0){ yNow = ((yMax - strtol(TextLine, &pEnd, 10)) - yMin) * 0.33333 * Scale;//Flip around y-axis coordinatePlot++; PrintMenue_03(FullFileName, coordinateCount, coordinatePlot, xNow, yNow, PlotStartTime, Scale); if(stopPlot == 0){ stopPlot =0; moveXY(xNow - currentPlotX, yNow - currentPlotY); currentPlotX = xNow; currentPlotY = yNow; // sprintf(TextLine, "xNow=%ld, yNow=%ld", xNow, yNow); // MessageText(TextLine, MessageX+1, MessageY+1, 0); // getch(); } else{ if(currentPlotDown == 1){ if(plotterMode == 1){ penMove(1); } currentPlotDown = 0; } } if(currentPlotDown == 0){ if(stopPlot == 0){ penMove(0); currentPlotDown = 1; } } ReadState = 2; i=0; TextLine[0]='\0'; } } } if(strcmp(TextLine, "<path") == 0){ if(currentPlotDown == 1){ penMove(1); currentPlotDown = 0; } ReadState = 2; i = 0; TextLine[0]='\0'; } if(a == '<'){ i = 1; TextLine[0] = a; TextLine[1] = '\0'; ReadState = 1; } } fclose(PlotFile); if(currentPlotDown == 1){ penMove(1); currentPlotDown = 0; } PrintMenue_03(FullFileName, coordinateCount, coordinatePlot, 0, 0, PlotStartTime,Scale); moveXY(-currentPlotX, -currentPlotY); currentPlotX = 0; currentPlotY = 0; while(kbhit()){ getch(); } MessageText("Finished! Press any key to return to main menu.", MessageX, MessageY, 0); getch(); break; } break; case 2: manualControl(); break; case 3: servoControl(); break; case 4: optoControl(); break; case 5: xyControl(); break; case 6: initalize(); break; case -1: default: system("reset"); quit=true; break; } } allSTOP(); endwin(); return 0; }