/* drive_param.h Mark Sibenac 97-3-7 Field Robotics Center Atacama Desert Trek This file defines constants and macros associated with the driving system. */ #ifndef __DRIVE_PARAM_H__ #define __DRIVE_PARAM_H__ // Real-Time Stuff //#define PID_FREQ (200) defined in robot.h #define DRIVE_TRAJ_FREQ (PID_FREQ) #define DRIVE_FREQ (1) // start of current feedback adc channels #define CURRENT_FB_OFFSET (32) // start of temperature fault di channels #define TEMP_OFFSET (0) // start of brake do channels #define BRAKE_OFFSET (13) // start of the limit switches for steering #define LIMIT_OFFSET (16) // start of the fault out signals from the amps di channels #define MOTOR_FAULT_OFFSET (28) // Kp,Ki,Kd are in 1000ths of Gain const int WHEEL_DEFAULT_KP = 110; const int WHEEL_DEFAULT_KI = 45; const int WHEEL_DEFAULT_KD = 600; const int WHEEL_DEFAULT_ILIM = 65535; const int WHEEL_DEFAULT_MAXPOSERR = 32000; const double WHEEL_DEFAULT_MAXCMD = 1.77; const int STEER_DEFAULT_KP = 200; const int STEER_DEFAULT_KI = 10; const int STEER_DEFAULT_KD = 5; const int STEER_DEFAULT_ILIM = 55000; const int STEER_DEFAULT_MAXPOSERR = 20000; const double STEER_DEFAULT_MAXCMD = 1.77; const float DEFAULT_STEER_INVRADINCR = 0.005; // 0.01 m const float DEFAULT_MAXSERVOERR = 0.05; // 0.5 m const float DEFAULT_STEER_JOY_GAIN = 0.004; const float DEFAULT_STEER_DELTA_JOY_GAIN = 10000.0; // These positions are not encoder ticks, they are radius * 44444.444444 const long DEFAULT_LEFT_DEPLOY_POS = 410058L; // 0.233m (inf rad) const long DEFAULT_RIGHT_DEPLOY_POS = 410058L; // 0.233m (inf rad) const long DEFAULT_LEFT_STOW_POS = 1210849L; // 0.692m (inf rad) const long DEFAULT_RIGHT_STOW_POS = 1210849L; // 0.692m (inf rad) const long DEFAULT_LEFT_TLEFT_POS = 610400L; // 13.734 (-2.54m rad) const long DEFAULT_RIGHT_TLEFT_POS = 292756L; // 6.587 (-2.54m rad) const long DEFAULT_LEFT_TRIGHT_POS = 292756L; // 6.587 (2.54m rad) const long DEFAULT_RIGHT_TRIGHT_POS = 610400L; // 13.734 (2.54m rad) const long DEFAULT_LEFT_POINT_POS = 150000; //97988L; // 2.205 0.056m (0 rad) const long DEFAULT_RIGHT_POINT_POS = 150000; //97988L; // 2.205 0.056m (0 rad) /* Assuming wheel diameter is 76cm, 2700rpm, Max Speed of robot is 49.24cm/s Assuming 160:1 harmonic instead of 100:1, max speed is 30.78cm/s WHEEL_TICKS_REV = 1024(encoder tick/rev) * 100harmonic * 48pinion / 22spurr WHEEL_MAX_VEL = 2700(motor rpm) / 100(harmonic) / 48(pinion) * 22(spurr) * 2PI(rads/rev) / 60(secs/min) = 1.2959 rads/s WHEEL_DEFAULT_ACCEL= (50cm/s)/(4s) = 12.5cm/s^2 = 685 rpm/s = 0.328776 rad/s^2 WHEEL_MAX_MOTOR_TICKS_SEC = 2700rpm / 60s/min * 1024 tick/rev = 46080 tick/s WHEEL_DEFAULT_MOTOR_ACCEL = 685 rpm/s / 60 s/min * 1024 tick/rev = 11691 ticks/s^2 */ const double ROBOT_MAX_VEL = 0.4924; // m/s const int ROBOT_MAX_TICKS_SEC = 46080; // ticks/sec /*const double ROBOT_MAX_VEL = 0.44; // m/s const int ROBOT_MAX_TICKS_SEC = 41165; // ticks/sec */ const double ROBOT_RADIANS_PER_METER = 2.63157894737; // 1/radius of wheel=.38m const double ROBOT_DEFAULT_ACCEL = 0.3; // m/s^2 const int ROBOT_DEFAULT_ACCEL_TICKS_SEC_SEC = 15000;// ticks/sec^2 const int ROBOT_TICKS_REV = 223418; // ticks const double ROBOT_POINT_TURN_RADIUS = 1.5; // meters /* Assuming wheel diameter is 76cm, 2700rpm, Max Speed of robot is 49.24cm/s Assuming 160:1 harmonic instead of 100:1, max speed is 30.78cm/s WHEEL_TICKS_REV = 1024(encoder tick/rev) * 100harmonic * 48pinion / 22spurr WHEEL_MAX_VEL = 2700(motor rpm) / 100(harmonic) / 48(pinion) * 22(spurr) * 2PI(rads/rev) / 60(secs/min) = 1.2959 rads/s WHEEL_DEFAULT_ACCEL= (50cm/s)/(4s) = 12.5cm/s^2 = 685 rpm/s = 0.328776 rad/s^2 WHEEL_MAX_MOTOR_TICKS_SEC = 2700rpm / 60s/min * 1024 tick/rev = 46080 tick/s WHEEL_DEFAULT_MOTOR_ACCEL = 685rpm/s / 60s/min * 1024tick/rev = 11691ticks/s^2 WHEEL_DEFAULT_MOTOR_ACCEL = 879rpm/s / 60s/min * 1024tick/rev = 15000ticks/s^2 */ const int WHEEL_TICKS_REV = 223418; // ticks const int WHEEL_MAX_MOTOR_TICKS_SEC = 46080; // ticks/s const int WHEEL_DEFAULT_MOTOR_ACCEL = 22040; // ticks/s^2 const double WHEEL_MAX_VEL = 1.29590696961; // rad/s const int WHEEL_MAX_VELI = 1296; // rad/s * 1000 //const double WHEEL_DEFAULT_ACCEL = 0.328776397843; // rad/s^2 /* Front Left Wheel Motor and Amp connection ports, and other constants */ #define WHEEL_FL_AMP (amps[MOTOR_DRIVE_FL]) #define WHEEL_FL_ENCODER (encoder[MOTOR_DRIVE_FL]) #define WHEEL_FL_CURRENT (adc[CURRENT_FB_OFFSET + MOTOR_DRIVE_FL]) #define WHEEL_FL_COMMAND (dac[MOTOR_DRIVE_FL]) #define WHEEL_FL_FAULT (di[MOTOR_FAULT_OFFSET + MOTOR_DRIVE_FL]) #define WHEEL_FL_TEMP (di[TEMP_OFFSET + MOTOR_DRIVE_FL]) #define WHEEL_FL_ENABLE (dout[MOTOR_DRIVE_FL]) /*#define WHEEL_FL_BRAKE (dout[BRAKE_OFFSET + MOTOR_DRIVE_FL])*/ #define WHEEL_FL_BRAKE (0) /* Front Right Wheel Motor and Amp connection ports, and other constants */ #define WHEEL_FR_AMP (amps[MOTOR_DRIVE_FR]) #define WHEEL_FR_ENCODER (encoder[MOTOR_DRIVE_FR]) #define WHEEL_FR_CURRENT (adc[CURRENT_FB_OFFSET + MOTOR_DRIVE_FR]) #define WHEEL_FR_COMMAND (dac[MOTOR_DRIVE_FR]) #define WHEEL_FR_FAULT (di[MOTOR_FAULT_OFFSET + MOTOR_DRIVE_FR]) #define WHEEL_FR_TEMP (di[TEMP_OFFSET + MOTOR_DRIVE_FR]) #define WHEEL_FR_ENABLE (dout[MOTOR_DRIVE_FR]) /*#define WHEEL_FR_BRAKE (dout[BRAKE_OFFSET + MOTOR_DRIVE_FR])*/ #define WHEEL_FR_BRAKE (0) /* Back Left Wheel Motor and Amp connection ports, and other constants */ #define WHEEL_BL_AMP (amps[MOTOR_DRIVE_BL]) #define WHEEL_BL_ENCODER (encoder[MOTOR_DRIVE_BL]) #define WHEEL_BL_CURRENT (adc[CURRENT_FB_OFFSET + MOTOR_DRIVE_BL]) #define WHEEL_BL_COMMAND (dac[MOTOR_DRIVE_BL]) #define WHEEL_BL_FAULT (di[MOTOR_FAULT_OFFSET + MOTOR_DRIVE_BL]) #define WHEEL_BL_TEMP (di[TEMP_OFFSET + MOTOR_DRIVE_BL]) #define WHEEL_BL_ENABLE (dout[MOTOR_DRIVE_BL]) #define WHEEL_BL_BRAKE (0) /* Back Right Wheel Motor and Amp connection ports, and other constants */ #define WHEEL_BR_AMP (amps[MOTOR_DRIVE_BR]) #define WHEEL_BR_ENCODER (encoder[MOTOR_DRIVE_BR]) #define WHEEL_BR_CURRENT (adc[CURRENT_FB_OFFSET + MOTOR_DRIVE_BR]) #define WHEEL_BR_COMMAND (dac[MOTOR_DRIVE_BR]) #define WHEEL_BR_FAULT (di[MOTOR_FAULT_OFFSET + MOTOR_DRIVE_BR]) #define WHEEL_BR_TEMP (di[TEMP_OFFSET + MOTOR_DRIVE_BR]) #define WHEEL_BR_ENABLE (dout[MOTOR_DRIVE_BR]) #define WHEEL_BR_BRAKE (0) /* 1" = 44,444 ticks STEER_TICKS_REV = 1024(encoder tick/rev) * 160harmonic * 48pinion / 22spurr STEER_MAX_VEL = 1350(motor rpm) / 100(harmonic) / 48(pinion) * 22(spurr) * 2PI(rads/rev) / 60(secs/min) = 0.64795 rads/s STEER_DEFAULT_ACCEL= (1"/s)/(15s) = .0667"/s^2 = 2.89 rps/s = 18.18 rad/s^2 STEER_MAX_MOTOR_TICKS_SEC = 1350rpm / 60s/min * 1024 tick/rev = 23040 tick/s STEER_DEFAULT_MOTOR_ACCEL = 2.89 rps/s * 1024 tick/rev = 2960 ticks/s^2 */ const int STEER_TICKS_REV = 223418; // ticks const int STEER_MAX_MOTOR_TICKS_SEC = 46080; // ticks/s const int STEER_DEFAULT_MOTOR_ACCEL = 23040; // ticks/s^2 const double STEER_MAX_VEL = 0.64795; // rad/s const int STEER_MAX_VELI = 648; // rad/s*1000 //const double STEER_DEFAULT_ACCEL = 13.635; // rad/s^2 const float RADIUS_STOWED = 300.0; const float RADIUS_POINT_CW = 1.0; const float RADIUS_POINT_CCW = -1.0; const float RADIUS_STRAIGHT = 100.0; const float RADIUS_DEPLOY = 200.0; const float RADIUS_TRIGHT = 2.54; // 100 inch radius const float RADIUS_TLEFT = -2.54; // 100 inch radius /* Left Steering Servo and Amp connection ports, and other constants */ #define STEER_LEFT_AMP (amps[MOTOR_STEER_L]) #define STEER_LEFT_ENCODER (encoder[MOTOR_STEER_L]) #define STEER_LEFT_CURRENT (adc[CURRENT_FB_OFFSET + MOTOR_STEER_L]) #define STEER_LEFT_COMMAND (dac[MOTOR_STEER_L]) #define STEER_LEFT_FAULT (di[MOTOR_FAULT_OFFSET + MOTOR_STEER_L]) #define STEER_LEFT_TEMP (di[TEMP_OFFSET + MOTOR_STEER_L]) #define STEER_LEFT_ENABLE (dout[MOTOR_STEER_L]) #define STEER_LEFT_BRAKE (0) /* Right Steering Servo and Amp connection ports, and other constants */ #define STEER_RIGHT_AMP (amps[MOTOR_STEER_R]) #define STEER_RIGHT_ENCODER (encoder[MOTOR_STEER_R]) #define STEER_RIGHT_CURRENT (adc[CURRENT_FB_OFFSET + MOTOR_STEER_R]) #define STEER_RIGHT_COMMAND (dac[MOTOR_STEER_R]) #define STEER_RIGHT_FAULT (di[MOTOR_FAULT_OFFSET + MOTOR_STEER_R]) #define STEER_RIGHT_TEMP (di[TEMP_OFFSET + MOTOR_STEER_R]) #define STEER_RIGHT_ENABLE (dout[MOTOR_STEER_R]) #define STEER_RIGHT_BRAKE (0) /* POTENTIOMETER SETTINGS */ #define STEER_RIGHT_POT_ADCNUM (5) #define STEER_LEFT_POT_ADCNUM (4) #define BOGIE_RIGHT_POT_ADCNUM (6) #define BOGIE_LEFT_POT_ADCNUM (6) #define STEER_RIGHT_POT (adc[STEER_RIGHT_POT_ADCNUM]) #define STEER_LEFT_POT (adc[STEER_LEFT_POT_ADCNUM]) #define BOGIE_RIGHT_POT (adc[BOGIE_RIGHT_POT_ADCNUM]) #define BOGIE_LEFT_POT (adc[BOGIE_LEFT_POT_ADCNUM]) #define LIMIT_LEFT_DEPLOY_DINUM (17) #define LIMIT_LEFT_STOW_DINUM (16) #define LIMIT_RIGHT_DEPLOY_DINUM (19) #define LIMIT_RIGHT_STOW_DINUM (18) #define LIMIT_LEFT_DEPLOY (di[LIMIT_LEFT_DEPLOY_DINUM]) #define LIMIT_LEFT_STOW (di[LIMIT_LEFT_STOW_DINUM]) #define LIMIT_RIGHT_DEPLOY (di[LIMIT_RIGHT_DEPLOY_DINUM]) #define LIMIT_RIGHT_STOW (di[LIMIT_RIGHT_STOW_DINUM]) #define ESTOP_BUTTON_DINUM (20) #define ESTOP_BUTTON (di[ESTOP_BUTTON_DINUM]) #define KILL_SWITCH_DINUM (6) #define KILL_SWITCH (di[KILL_SWITCH_DINUM]) #endif /* __DRIVE_PARAM_H__ */