void pre_auton() { initializeDisplay(); initializeGyro(); //Uncomment when we start using Gyro Sensor initializePID(); initializeAutonomous(); while (bIfiRobotDisabled); //Keep the message displayed while robot is disabled }
void AuRo666Plugin::Load(physics::ModelPtr model_ptr, sdf::ElementPtr sdf_element_ptr) { this->model_ptr = model_ptr; initializePID(sdf_element_ptr); /* ********************* Assigning the joint pointers *********************** */ // TODO: Replace such strings with const vars joints[front_left_yaw_id] = model_ptr->GetJoint("front_left_joint"); joints[front_right_yaw_id] = model_ptr->GetJoint("front_right_joint"); joints[back_left_velocity_id] = model_ptr->GetJoint("back_left_joint"); joints[back_right_velocity_id] = model_ptr->GetJoint("back_right_joint"); last_update_time = model_ptr->GetWorld()->GetSimTime(); connection_ptr = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&AuRo666Plugin::updateState, this)); }
/*************************************************************************** main program ***************************************************************************/ int main(int argc, char* argv[]) { /*************************************************************************** Initialize variables ***************************************************************************/ int ret; unsigned int n = 0; //----- SET SPI MODE ----- //SPI_MODE_0 (0,0) CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge //SPI_MODE_1 (0,1) CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge //SPI_MODE_2 (1,0) CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge //SPI_MODE_3 (1,1) CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge uint8_t lcd_spi_config = SPI_MODE_0; uint8_t ads_spi_config = SPI_MODE_1; int ShmID; pid_t pid_stdin; pid_t pid_pwm; Controller *ctlPTR; //this is the key structure that holds all the shared data. /*************************************************************************** setup shared memory ***************************************************************************/ ShmID = shmget(IPC_PRIVATE, sizeof(Controller), IPC_CREAT | 0666); if (ShmID < 0) { fprintf(stderr,"reflowControl Exiting - could not initialize shared memory area (shmget)\n"); exit(1); } ctlPTR = (Controller *) shmat(ShmID, NULL, 0); //assign controller if ((void *) ctlPTR == (void *) -1) { fprintf(stderr,"reflowControl Exiting - could not attach to shared memory area (shmat)\n"); exit(1); } initializePID(ctlPTR); /*************************************************************************** setup ADS, LCD, other GPIO ***************************************************************************/ sleep(2); //sleep for 2 seconds at start to make sure Node program has started listening for IO streams //make sure output is not buffered (this program generally a child to a nodejs program) and set signal handler setbuf(stdout, NULL); //set stdout to not buffer output setbuf(stderr, NULL); //set stderr to not buffer output signal(SIGINT, sig_handler_main); //capture signal interrupt ^C to cleanly shutdown signal(SIGTERM, sig_handler_main); //capture process termination signal to cleanly shutdown //Initialize GPIO - see http://elinux.org/RPi_Low-level_peripherals#C_2 // Set up gpi pointer for direct register access setup_io(); // Set GPIO pin 23 SSR1_GPIO to output INP_GPIO(SSR1_GPIO); // must use INP_GPIO before we can use OUT_GPIO OUT_GPIO(SSR1_GPIO); GPIO_CLR = 1<<SSR1_GPIO; //make sure output is turned off // Set GPIO pin 22 BUZZER_GPIO to output INP_GPIO(BUZZER_GPIO); // must use INP_GPIO before we can use OUT_GPIO OUT_GPIO(BUZZER_GPIO); GPIO_SET = 1<<BUZZER_GPIO; //make sure output is turned off - Buzzer is PNP, so pin high is buzzer off. // Set GPIO pin 17 LCD_RS_GPIO to output INP_GPIO(LCD_RS_GPIO); // must use INP_GPIO before we can use OUT_GPIO OUT_GPIO(LCD_RS_GPIO); // Initialize LCD ret = spi_open(&lcd_fd, 0, lcd_spi_config); //initialize lcd_fd, 0 means device 0 (LCD), SPI_MODE_0 if (ret != 0) { fprintf(stderr,"reflowControl Exiting - Could not initialize LCD\n"); exit(1); } lcd_init(); lcd_clear(); // LCD clear lcd_display_string(0,"Reflow Start", &ctlPTR->spiSemaphore); // display startup message // Initialize ADS Thermocouple chip ret=spi_open(&ads_fd, 1, ads_spi_config); if (ret != 0) { fprintf(stderr,"reflowControl Exiting - could not initialize ADS thermocouple chip\n"); exit(1); } /*************************************************************************** fork child process to manage SSR ***************************************************************************/ pid_pwm = fork(); switch(pid_pwm) { case -1: fprintf(stderr,"reflowControl Exiting - could not fork a child process for PWM (fork)\n"); exit(1); break; case 0: //child process child_main(ctlPTR); break; default: //main process printf("{\"item\": \"childProcess\", \"value\": \"PID_PWM\", \"pid\": %d}\n", pid_pwm); /*************************************************************************** fork child process to manage stdin ***************************************************************************/ pid_stdin = fork(); switch(pid_stdin) { case -1: fprintf(stderr,"reflowControl Exiting - could not fork a child process for stdin (fork)\n"); exit(1); break; case 0: //child process stdin_main(ctlPTR); break; default: //main process printf("{\"item\": \"childProcess\", \"value\": \"PID_STDIN\", \"pid\": %d}\n", pid_stdin); while (1) { n++; delay_ms(MS_SAMPLES); sampleTemp(&ctlPTR->tc1, &ctlPTR->spiSemaphore); if (n >= NSAMPLES) { ctlPTR->tc1.value = sampleMean(&ctlPTR->tc1); ctlPTR->tc1.stdev = sampleStddev(&ctlPTR->tc1); printMeasurementJSON(&ctlPTR->tc1); ctlPTR->tc1.n = 0; ctlPTR->pv = ctlPTR->tc1.value; n = 0; } } break; } } //exit kill(pid_pwm, SIGKILL); shutdown(); return(0); }
int main(int argc, const char **argv) { int i, a; double t; int i2c_fd; coordData_t * waypoints; int numWaypts = NUM_WAYPOINTS; double w[] = WAYPOINT_ARRAY; pthread_t thread; //Set signals to handle signal(SIGINT, &sighandle); signal(SIGTERM, &sighandle); signal(SIGABRT, &sighandle); //allocate structs if((hands = malloc(sizeof(api_HANDLES_t))) == NULL) { fprintf(stderr,"Error calling malloc\n"); return -1; } if((filt = malloc(sizeof(FilterHandles_t))) == NULL) { fprintf(stderr,"Error calling malloc\n"); return -1; } if((pids = malloc(sizeof(pidHandles_t))) == NULL) { fprintf(stderr,"Error calling malloc\n"); return -1; } if((waypoints = malloc(sizeof(coordData_t) * numWaypts)) == NULL) { fprintf(stderr,"Error calling malloc\n"); return -1; } if((posData = malloc(sizeof(roboPos_t))) == NULL) { fprintf(stderr,"Error calling malloc\n"); return -1; } for(i=0; i<numWaypts; i++) { waypoints[i].X = w[i*2]; waypoints[i].Y = w[i*2+1]; } // allocate devices if((hands->c = create_create(COMPORT)) == NULL) { fprintf(stderr,"Error connecting create\n"); return -1; } if((hands->t = turret_create()) == NULL) { fprintf(stderr,"Error connecting turrett\n"); return -1; } //open serial com port if(create_open(hands->c, FULLCONTROL) < 0) { fprintf(stderr,"Failed to open create\n"); return -1; } //open i2c device if(turret_open(hands->t) < 0) { fprintf(stderr,"Failed to connect to robostix\n"); return -1; } //initialize the robostix board turret_init(hands->t); //Initialize Filter structs filt->sonar0 = initializeFilter(FILT_SONAR); filt->sonar1 = initializeFilter(FILT_SONAR); filt->ir0 = initializeFilter(FILT_IR); filt->ir1 = initializeFilter(FILT_IR); //Initialize PID structs pids->trans = initializePID(TRANS_PID); pids->angle = initializePID(ANGLE_PID); pids->sonar = initializePID(SONAR_PID); pids->angleT = initializePID(ANGLET_PID); // process command line args i = 1; while( i < argc ) { if(!strcmp(argv[i], "-w")) { //Accept command line waypoints if(argc >= i+1) { numWaypts = atoi(argv[++i]); if(argc >= i+(2*numWaypts)) { free(waypoints); if((waypoints = malloc(sizeof(coordData_t) * numWaypts)) == NULL) { fprintf(stderr,"Error calling malloc\n"); return -1; } for(a=0; a<numWaypts; a++) { waypoints[a].X = atoi(argv[++i]); waypoints[a].Y = atoi(argv[++i]); } i++; } else { printf("Error using [-w].\n\tUsage: -w numPoints X1 Y1 X2 Y2...\n"); } } else { printf("Error using [-w].\n\tUsage: -w numPoints X1 Y1 X2 Y2...\n"); } } else { printf("Unrecognized option [%s]\n",argv[i]); return -1; } } //Create thread to monitor robot movement pthread_create(&thread, NULL, mapRobot, (void *)hands); // robot is ready #ifdef DEBUG printf("All connections established, ready to go\n"); #endif //Rotate the Servo turret_SetServo(hands->t, T_ANGLE); for(i=0; i<FILT_IR_SAMPLES; i++) { filterIR(hands, filt, &t, &t); #ifndef USE_IR_MODE filterSonar(hands, filt, &t, &t); #endif } //Iterate over all of the waypoints for(i=0; i<numWaypts; i++) { #ifdef DEBUG printf("Waypoint %d (%f,%f).\n",i+1,waypoints[i].X,waypoints[i].Y); #endif Move(hands,filt,pids,waypoints[i].X,waypoints[i].Y); #ifdef DEBUG printf("\nArrived at waypoint %d (%f,%f).\n\n", i+1,waypoints[i].X,waypoints[i].Y); #endif } // Shutdown and tidy up. Close all of the proxy handles cleanup(); return 0; }
//#subregion targeting void initializeTargetingPID(motorGroup *group, float kP, float kI, float kD, int errorMargin=100, int minSampleTime=10, bool useTimeCorrection=true, int integralMax=127) { //TODO: integralMax DOES NOT WORK with autoStillSpeeding initializePID(group->posPID, 0, kP, kI, kD, minSampleTime, useTimeCorrection, integralMax); group->waitErrorMargin = errorMargin; group->autoStillSpeeding = false; }