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