Exemple #1
0
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);
}
Exemple #4
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;
}