Exemplo n.º 1
0
int main(int argc,char **argv) {
  int i; //iterator
  pid *x,*y,*z; //structs representing 3 pid controllers
  imu_t *imu;
  fd_set net_set;
  double oldmag,magdiff;
  struct timeval timeout;
  int axes[4]={0,0,0,0};
  int x_adjust,y_adjust,z_adjust;
  int sock = init_net();
  register_int();
  x=init_pid(3500,000,000);
  y=init_pid(3400,000,1000);
  z=init_pid(000,000,000);
  timeout.tv_sec=0;
  timeout.tv_usec=REFRESH_TIME;
  for(i=0;i<4;i++) { //initialize PWM modules
    motors[i]=init_pwm(i);
  }
  imu = init_imu();
  update_imu(imu);
  oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
  while(1) {
    ////printf("loop\n");
    FD_ZERO(&net_set);
    FD_SET(sock,&net_set);
    if(select(sock+1,&net_set,(fd_set *) 0,(fd_set *) 0, &timeout)==1) {
      ////printf("reading from the socket\n");
      //read from sock
      if(!update_axis(sock,axes)) {
        break;
      }
    } else {
      ////printf("reading from the imu\n");
      //update imu
      update_imu(imu);
      timeout.tv_sec=0;
      timeout.tv_usec=REFRESH_TIME;
      magdiff=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE-oldmag;
      if(magdiff>300) {
        magdiff-=360;
      }
      if(magdiff<-300) {
        magdiff+=360;
      }
      x_adjust = update_pid(x,0/*axes[1]*.00061*/,imu->mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE);
      y_adjust = update_pid(y,0/*axes[2]*.00061*/,imu->mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE);
      //printf("X axis value: %lf\n",imu->mpu.fusedEuler[VEC3_X]*RAD_TO_DEGREE);
      //printf("Y axis value: %lf\n",imu->mpu.fusedEuler[VEC3_Y]*RAD_TO_DEGREE);
      z_adjust = update_pid(z,axes[3]*.00061,magdiff);
      oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
      set_duty(motors[0],900000+axes[0]*16-z_adjust-y_adjust+x_adjust);
      set_duty(motors[1],900000+axes[0]*16+z_adjust-y_adjust-x_adjust);
      set_duty(motors[2],900000+axes[0]*16-z_adjust+y_adjust-x_adjust);
      set_duty(motors[3],900000+axes[0]*16+z_adjust+y_adjust+x_adjust);
    }
  }
  exit_fxn(0);
}
int umain(){
    robot_id = 1;
    copy_objects();
    copy_objects();
    field_state.curr_loc.x = (int)(game.coords[0].x * VPS_RATIO);
    field_state.curr_loc.y = (int)(game.coords[0].y * VPS_RATIO);
    field_state.curr_angle = (((float)game.coords[0].theta) * 180.0) / 2048;
    gyro_set_degrees(field_state.curr_angle);

        
    initialize();
    
    /*for(int sextant = 0; sextant < 6; sextant++){
        printf("TPX: %d, TPY: %d, LPX: %d, LPY: %d\n", get_territory_pivot_point_loc(sextant).x, get_territory_pivot_point_loc(sextant).y, lever_pivot_points[2*sextant], lever_pivot_points[2*sextant + 1]);
    }*/
    init_pid(&field_state.circle_PID, KP_CIRCLE, KI_CIRCLE, KD_CIRCLE, &get_angle_error_circle, &update_circle_velocities);
    field_state.circle_PID.enabled = true;
    field_state.circle_PID.goal = 0;
    
    //printf("OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT));
    
    while(1){
        update_field();
        accelerate();
        decelerate();
        if(field_state.pid_enabled)
            update_pid(&field_state.circle_PID);
        if(get_time() > 119000){
            
        }
        //printf("Sextant: %d, OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", get_current_sextant(field_state.curr_loc), field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT));

    }
    return 0;
}
Exemplo n.º 3
0
void resetPID(float target) {
    init_pid(&driver,
            DRIVE_KP,
            DRIVE_KI,
            DRIVE_KD,
            &runGetError,
            &turnAndDrive);
    driver.goal = 0;
    driver.enabled = 1;
}
Exemplo n.º 4
0
void PIDReset() {
    init_pid(&ForwardPID,
            (float)KP,
            (float)KD,
            (float)KI,
            &getError,
            &ForwardApplyMV);
    ForwardPID.goal = 0.0;
    ForwardPID.enabled = 1;

    init_pid(&RingPID,
            RINGKP,
            RINGKD,
            RINGKI,
            &getStrengthError,
            &applyNearbyScale);
    RingPID.goal = 0.0;
    RingPID.enabled = 1;
}
Exemplo n.º 5
0
int umain(void) {
    create_thread(&display_angle, 64, 0, "display angle");

    struct pid_controller pid;
    init_pid (&pid, 1, 0, 0, &read_angle, &set_turn);
    pid.goal = 90;
    pid.enabled = 1;

    dispatch_pid (&pid, 0.01, 0);
    printf("pid done.\n");

    return 0;
}
Exemplo n.º 6
0
void PIDInit() {
    SetTargetVelocity(TARGET_VELOCITY);

    printf("\n");

    init_pid(&ForwardPID,
            (float)KP,
            (float)KI,
            (float)KD,
            &getError,
            &ForwardApplyMV);
    ForwardPID.goal = 0.0;
    ForwardPID.enabled = 1;

    init_pid(&RingPID,
            RINGKP,
            RINGKD,
            RINGKI,
            &getStrengthError,
            &applyNearbyScale);
    RingPID.goal = 0.0;
    RingPID.enabled = 1;
}
Exemplo n.º 7
0
void init() {
    unsigned short vid_pid[][2] = {
        {0x0c45, 0x7403},
        {0x0c45, 0x7404},
        {0x413d, 0x2107},
    };
    int i = 0;
    for (i = 0 ; i < sizeof(vid_pid) / sizeof(vid_pid[0]) ; i++) {
        init_pid(vid_pid[i][0], vid_pid[i][1]);
        if (dev != NULL) {
            break;
        }
    }
    if (dev == NULL) {
        fatal("Cannot find footswitch with one of the supported VID:PID.\nCheck that the device is connected and that you have the correct permissions to access it.");
    }
}
Exemplo n.º 8
0
static void lmdbg_startup (void)
{
	if (real_malloc){
		/* already initialized */
		return;
	}

	init_fun_ptrs ();
	init_log ();
	init_pid ();
	init_st_range ();
	print_sections_map ();
	print_progname ();
	init_environment ();
	init_enabling_timeout ();

	if (log_filename != NULL && enabling_timeout == 0)
		enable_logging ();
	else if (enabling_timeout == -1)
		set_sigusr1_handler ();
}
Exemplo n.º 9
0
int
runonce_pids_create( void )
{
  DIR *fproc;

  runonce_pids.array = mas_malloc( runonce_pids.size * sizeof( prec_t ) );
  memset( runonce_pids.array, 0, runonce_pids.size * sizeof( prec_t ) );

  fproc = opendir( "/proc" );
  if ( fproc )
  {
    /* int readdir_r(DIR *dirp, struct dirent *entry, struct dirent **result); */
    struct dirent de;
    struct dirent *pde;

    while ( 0 == readdir_r( fproc, &de, &pde ) && pde )
    {
      int isnum = 1;

      for ( const char *p = pde->d_name; *p; p++ )
      {
        if ( !isdigit( *p ) )
        {
          isnum = 0;
          break;
        }
      }
      if ( isnum )
      {
        init_pid( pde->d_name );
      }
    }
    closedir( fproc );
  }
  return 0;
}
Exemplo n.º 10
0
int main(void)
{
	//Sväng inte
	turn = 0;
	
	//Initiera spi, pwm och display
	spi_init();
	pwm_init();
	init_display();
	update();
	
	claw_out();
	_delay_ms(500);
	claw_in();	
	
	//Aktivera global interrupts
	sei();
	
	//Initiera regulator
	clear_pid();	
	init_pid(80, -80);
	//update_k_values(40, 12, 22);
	update_k_values(40, 170, 20);
	
	// Pekare till aktuell position i bufferten
	tmp_sensor_buffer_p = 0x00;
	
	// Flagga som avgör huruvida vi är i början av meddelande	
	sensor_start = 1;
	
	// Anger aktuell längd av meddelandet				
	tmp_sensor_buffer_len = 0x00;
	
	//Initiera standardsträng på display		
	init_default_printf_string();
	clear_screen();
	update();
	
	
	while(1)
	{
		uint8_t has_comm_data, has_sensor_data, comm_data, sensor_data;
	
		do_spi(&has_comm_data, &has_sensor_data, &comm_data, &sensor_data);
		
		//Undersök och hantera meddelanden från slavarna
		if(has_comm_data) decode_comm(comm_data);
		if(has_sensor_data) decode_sensor(sensor_data);
		
		//Vid manuell sväng eller 180 grader måste make_turn anropas 		
		if(!autonomous || turning_180)
		{
			if(turn_dir) 
			{
				make_turn_flag = 1;
				make_turn(turn_dir);
				
				if(!make_turn_flag) 
				{
					turn_dir = 0;
					stop_motors();
				}				
			}
		}			
		
		//Kör regulatorn
		if (regulator_enable)
		{
			regulator(sensor_buffer[IR_RIGHT_FRONT] - sensor_buffer[IR_RIGHT_BACK], 
					  sensor_buffer[IR_LEFT_FRONT] - sensor_buffer[IR_LEFT_BACK], 
					  sensor_buffer[IR_RIGHT_FRONT] - sensor_buffer[IR_LEFT_FRONT], 
					  sensor_buffer[IR_RIGHT_BACK] - sensor_buffer[IR_LEFT_BACK]);
			regulator_enable = 0;
		}	
	}
}
Exemplo n.º 11
0
void Control(T_float_angle *att_in,T_int16_xyz *gyr_in, T_RC_Data *rc_in, u8 armed)
{                          //加速度             角速度
	static u8 first =1;
	T_float_angle angle;	
	#ifndef USE_USART1
	if(first)
	{
	 init_pid();
		first=0;
	}
	#endif
	
	//**************roll轴指向***********************************************************
  angle.rol = att_in->rol - (rc_in->ROLL-1500)/25;//误差
	if(rc_in->THROTTLE<1200)
		rol_i=0;
	else
	  rol_i += angle.rol;//误差和
	LIMIT(-2000, rol_i,2000);//积分限幅
	PID_ROL.pout = PID_ROL.P * angle.rol;      //比例项 = 比例*误差
	PID_ROL.dout = -PID_ROL.D * gyr_in->Y;     //微分项 = 微分*变化量
	//PID_ROL.iout = PID_ROL.I * PID_ROL.dout; //积分项 = 积分*误差和
	PID_ROL.iout = PID_ROL.I * rol_i;

	PID_ROL.OUT =  (-PID_ROL.pout)-PID_ROL.iout +PID_ROL.dout;//
    //**************pith轴指向***********************************************************
	angle.pit = att_in->pit + (rc_in->PITCH-1500)/25;
	if(rc_in->THROTTLE<1200)
		pit_i=0;
	else
	  pit_i += angle.pit;//误差和
	LIMIT(-2000, pit_i,2000);
	PID_PIT.pout = PID_PIT.P * angle.pit;   //比例项 = 比例*误差
	PID_PIT.dout = PID_PIT.D * gyr_in->X;   //微分项 = 微分*变化量
	PID_PIT.iout = PID_PIT.I * pit_i;       //积分项 = 积分*误差和
	//PID_PIT.iout = PID_PIT.I *PID_PIT.dout ;

	PID_PIT.OUT = PID_PIT.pout + PID_PIT.iout + PID_PIT.dout;
	//**************yaw轴指向***********************************************************
	if(rc_in->YAW<1400||rc_in->YAW>1600)
	{gyr_in->Z=gyr_in->Z+(rc_in->YAW-1500)*2;}
	if(rc_in->THROTTLE<1200)
		yaw_p=0;
	else 
		yaw_p+=gyr_in->Z*0.0609756f*0.002f;// +(Rc_Get.YAW-1500)*30
	
	if(yaw_p>20)
		yaw_p=20;
	if(yaw_p<-20)
		yaw_p=-20;
	PID_YAW.pout = PID_YAW.P*yaw_p;
	PID_YAW.dout = PID_YAW.D * gyr_in->Z;		
	PID_YAW.iout = PID_YAW.I *PID_YAW.dout ;

	PID_YAW.OUT = PID_YAW.pout + PID_YAW.iout + PID_YAW.dout;

    //**************电机控制***********************************************************
	if(rc_in->THROTTLE>1200&&armed)//armed判断解锁
	{                                       //横滚            //俯仰        //航向
//		Moto_PWM_1 = rc_in->THROTTLE - 1000  - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
//		Moto_PWM_2 = rc_in->THROTTLE - 1000  - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
//		Moto_PWM_3 = rc_in->THROTTLE - 1000  + PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
//		Moto_PWM_4 = rc_in->THROTTLE - 1000  + PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
		Moto_PWM_1 = rc_in->THROTTLE - 1000 + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT;//反逻辑 电机下沉端用加号
		Moto_PWM_2 = rc_in->THROTTLE - 1000 + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT;
		Moto_PWM_3 = rc_in->THROTTLE - 1000 - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT;
		Moto_PWM_4 = rc_in->THROTTLE - 1000 - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT;
	}
	else
	{
		Moto_PWM_1 = 0;
		Moto_PWM_2 = 0;
		Moto_PWM_3 = 0;
		Moto_PWM_4 = 0;
	}
	//**************更新PWM***********************************************************
	Moto_PwmRflash(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4);
}
Exemplo n.º 12
0
/*******************************************************************************
* FUNCTION NAME: User_Initialization
* PURPOSE:       This routine is called first (and only once) in the Main function.  
*                You may modify and add to this function.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Initialization (void)
{
  Set_Number_of_Analog_Channels(SIXTEEN_ANALOG);    /* DO NOT CHANGE! */

/* FIRST: Set up the I/O pins you want to use as digital INPUTS. */
  digital_io_01 = digital_io_02 = digital_io_03 = digital_io_04 = INPUT;
  digital_io_05 = digital_io_06 = digital_io_07 = digital_io_08 = INPUT;
  digital_io_09 = digital_io_10 = digital_io_11 = digital_io_12 = INPUT;
  digital_io_13 = digital_io_14 = digital_io_15 = digital_io_16 = INPUT;
  digital_io_18 = INPUT;  /* Used for pneumatic pressure switch. */
    /* 
     Note: digital_io_01 = digital_io_02 = ... digital_io_04 = INPUT; 
           is the same as the following:

           digital_io_01 = INPUT;
           digital_io_02 = INPUT;
           ...
           digital_io_04 = INPUT;
    */

/* SECOND: Set up the I/O pins you want to use as digital OUTPUTS. */
  digital_io_17 = INPUT;    /* Example - Not used in Default Code. */

/* THIRD: Initialize the values on the digital outputs. */
//  rc_dig_out17 = 0;

/* FOURTH: Set your initial PWM values.  Neutral is 127. */
  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;

/* FIFTH: Set your PWM output types for PWM OUTPUTS 13-16.
  /*   Choose from these parameters for PWM 13-16 respectively:               */
  /*     IFI_PWM  - Standard IFI PWM output generated with Generate_Pwms(...) */
  /*     USER_CCP - User can use PWM pin as digital I/O or CCP pin.           */
  Setup_PWM_Output_Type(IFI_PWM,IFI_PWM,IFI_PWM,IFI_PWM);

  /* 
     Example: The following would generate a 40KHz PWM with a 50% duty cycle on the CCP2 pin:

         CCP2CON = 0x3C;
         PR2 = 0xF9;
         CCPR2L = 0x7F;
         T2CON = 0;
         T2CONbits.TMR2ON = 1;

         Setup_PWM_Output_Type(USER_CCP,IFI_PWM,IFI_PWM,IFI_PWM);
  */

  /* Add any other initialization code here. */
 
  Init_Serial_Port_One();
  Init_Serial_Port_Two();

#ifdef TERMINAL_SERIAL_PORT_1    
  stdout_serial_port = SERIAL_PORT_ONE;
#endif

#ifdef TERMINAL_SERIAL_PORT_2    
  stdout_serial_port = SERIAL_PORT_TWO;
#endif

	Initialize_Encoders();
	//I EXPECT INITIALIZATION VALUES
	Reset_Encoder_1_Count(-153);
	Reset_Encoder_2_Count(-415);

	//start comment
	Initialize_Gyro();
    Initialize_ADC();
	//end comment

	init_pid(&arm, 100, 0, 0, 120, 35);  // 275 0 0
	init_pid(&wrist, 33, 0, 0, 20, 80); //45 30 0
	init_pid(&Mr_Roboto, 55, 0 , 0, 100, 25);
	init_pid(&robot_dist, 95, 0, 0, 100, 8);
	init_pid(&gyro_c, 20, 0, 0, 100, 8);

  Putdata(&txdata);            /* DO NOT CHANGE! */

//  ***  IFI Code Starts Here  ***
//
//  Serial_Driver_Initialize();

  printf("IFI 2006 User Processor Initialized ...\r");  /* Optional - Print initialization message. */

  User_Proc_Is_Ready();         /* DO NOT CHANGE! - last line of User_Initialization */
}
Exemplo n.º 13
0
/*********************************************************************
  Function:        int main(void)

  PreCondition:    None.
 
  Input:           None.

  Output:          None.

  Side Effects:    None.

  Overview:        main function of the application. Peripherals are 
                   initialized.

  Note:            None.
********************************************************************/
int main(void) 
{
	int cs;
	// vars used for detection of incremental motion
	unsigned short new_cmd,last_cmd, new_fb,last_fb;

	setup_io();			// make all i/o pins go the right dir
	STATUS_LED = 0;		// led on

	setup_uart();		// setup the serial interface to the PC

    setup_TMR1();       // set up 1ms timer
	IEC0bits.T1IE = 1;  // Enable interrupts for timer 1
   						// needed for delays in following routines

	// 1/2 seconds startup delay 
	timer_test = 5000;		
	while ( timer_test );

	printf("\r\nPowerup..i/o...uart...timer...");	

	setup_pwm();		// start analog output
	set_pwm(0.0); 
	printf("pwm...");

	init_pid();
	printf("pid...");

    setup_encoder();    // 16 bit quadrature encoder module setup
	printf("encoder...");

    setup_capture();    // 2 pins with quadrature cmd from PC
	printf("capture...");

	printf("done\r\n");

	// some junk for the serial channel
	printf("%s%s\n\r",CPWRT,VERSION);

	STATUS_LED = 1;		// led off when init finished

	// restore config from eeprom
	// Read array named "setupEE" from DataEEPROM and place 
	// the result into array in RAM named, "setup" 
	restore_setup();
	cs = calc_cksum(sizeof(pid)/sizeof(int),(int*)&pid);
	if ( cs )
	{
		// opps, no valid setup detected
		// assume we are starting from a new box
		printf("No valid setup found in EEPROM\r\n");
		init_pid();
	}
	else
	{
		printf("Using setup from eeprom.. ? for help\r\n");
		print_tuning();
	}
    printf("using %fms servo loop interval\r\n",pid.ticksperservo * 0.1);

//	BLOCK OF TEST ROUTINES FOR HARDWARE DEBUGGING		
//	test_pwm_interface();		// play with opa549 hardware
//	test_pc_interface();		// echo cmded posn to serial port
//  test_pid_interface();		// test pid loop operation

	new_cmd = last_cmd = new_fb = last_fb = 0;

	while (1)
	{
		if ( do_servo )		// check and see if timer 1 has asked for servo calcs to be run
		{
			do_servo = 0;
			if (SVO_ENABLE)
			{
				if ( pid.enable == 0 )	// last loop, servo was off
				{
				    set_pwm( 0.0 );
					printf("servo-enabled\r\n>");
					pid.enable = 1;
					// make sure we dont move on enabling
					cmd_posn = POSCNT;		// make 16bit incr registers match
					pid.command = 0L;		// make 32 bit counter match
					pid.feedback = 0L;
					// make the 1ms loop temps match
					new_cmd = last_cmd = new_fb = last_fb = 0;
					pid.error_i = 0.0;		// reset integrator
				}
                // we can time the servo cycle calcs by scoping the PID_ACTIVE pin
			    PID_ACTIVE = 1;			// seems to take about 140us
			    new_cmd = cmd_posn;		// grab current cmd from pc
			    new_fb = POSCNT;		// grab current posn from encoder

			    pid.command  += (long int)((short)(new_cmd - last_cmd));
			    pid.feedback += (long int)((short)(new_fb  - last_fb ));
			    last_cmd = new_cmd;
			    last_fb = new_fb;

			    calc_pid();

			    // check for a drive fault ( posn error > allowed )
			    if (( pid.maxerror > 0.0 ) && 
				    ( fabs(pid.error) > pid.maxerror ))
			    {
				    short temp = SVO_ENABLE;
				    set_pwm( 0.0 );
				    while (1)	// trap here until svo disabled or pwr cycle
				    {
					    // reset integrator as it may have wound up
					    pid.error_i = 0.0;
					    printf("drive fault... maxerror exceeded\r\n");
					    STATUS_LED = 0;	timer_test = 2500; while ( timer_test );
					    STATUS_LED = 1;	timer_test = 2500; while ( timer_test );
					    STATUS_LED = 0;	timer_test = 2500; while ( timer_test );
					    STATUS_LED = 1;	timer_test = 2500; while ( timer_test );
					    if (temp != SVO_ENABLE) 
						    break;
				    }
			    }
			    else
			    {
				    set_pwm(pid.output);	// update motor drive
			    }
			    PID_ACTIVE = 0;			// i/o pin for timing pid calcs
			}
			else
			{
				if ( pid.enable == 1 )	// last loop servo was active
				{
				    set_pwm( 0.0 );
					pid.enable = 0;
					printf("servo-disabled\r\n>");
					// extra delay keeps us faulted for min 1 sec to let mechanicals settle
					STATUS_LED = 1;	timer_test = 10000; while ( timer_test );
				}
			}
		}

		// look for serial cmds
		// doing this while svo is enabled will cause bumps in servo loop
		// because of serial i/o time ( unless we get smart and move svo loop
		// into an isr )
		if ( rxrdy )
			process_serial_buffer();

		if (pid.limit_state)			// show we are at drive limit(error)
			STATUS_LED = 0;
		else
			STATUS_LED = 1;
	}
	// to keep compiler happy....
	return 0;
}
Exemplo n.º 14
0
void reset_pid_controller(float goal) {
	init_pid(&controller, KP, KI, KD, NULL, NULL);
	controller.goal = goal;
}