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;
}
void resetPID(float target) {
init_pid(&driver,
DRIVE_KP,
DRIVE_KI,
DRIVE_KD,
&runGetError,
&turnAndDrive);
driver.goal = 0;
driver.enabled = 1;
}
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;
}
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;
}
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;
}
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.");
}
}
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 ();
}
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;
}
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;
}
}
}
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);
}
/*******************************************************************************
* 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 */
}
/*********************************************************************
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;
}
void reset_pid_controller(float goal) {
init_pid(&controller, KP, KI, KD, NULL, NULL);
controller.goal = goal;
}
