void system_init(void){
TA = 0xAA; //time access
TA = 0x55; //time access
EWT = 0; //off watchdog timer
PMR = 0x83; //use on chip XRAM
CKMOD = 0x38; //timer run at x 1 mode
P0 = 0xFF; //port pin initialization
P1 = 0xFF;
P2 = 0xFF;
P3 = 0xFF;
TMOD = 0x21; //timer 1, 8-bit auto reload
SCON1 = 0x52;
SCON0 = 0x52;
/**Interrupt Enable**/
IE = GLOBAL_INT | TIMER2_INT | EXT0_INT | SP1_INT;
/*Interrupt Priority*/
IP1 = EXT0_PRT | SP1_PRT;
IP0 = EXT0_PRT | TIMER2_PRT;
IT0 = 1; //external interrupt edge trigger
IT1 = 1;
TH1 = BAUDRATE_TIMER;
T2CON = 0x00; //timer 2 16 bit auto reload
RCAP2H = 0xA9; //timer value set to count 1 ms
RCAP2L = 0x9A;
TR1 = 1; //start timer 1
TR2 = 1; //start timer 2
delay_ms(500);
init_lcd();
lcd_print("KT_AYE\n1071118793\nRobotic & automation\nSW1 to start\n");
ResetPID();
Pk = 10;
Ik = 3;
Dk = 3;
XPos = 0;
YPos = 0;
XDistance = 240;
YDistance = 240;
MaxSpeed = 3000;
DesiredRange = 850;
DelayCounter = 0;
EncoderFeedback = 0;
TotalEncoderFeedback = 0;
RobotLength = 43;
serialStart = OFF;
}
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
ShooterArm::ShooterArm() : PIDSubsystem("ShooterArm", 0.75, 0.16, 0.7) {
SetAbsoluteTolerance(0.3);
GetPIDController()->SetContinuous(false);
LiveWindow::GetInstance()->AddActuator("ShooterArm", "PIDSubsystem Controller", GetPIDController());
GetPIDController()->SetOutputRange(-1.0, 0.75);
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
// BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
shooterArmPot = RobotMap::shooterArmshooterArmPot;
shooterArmMotor = RobotMap::shooterArmshooterArmMotor;
fiveVoltMeasure = RobotMap::shooterArmFiveVoltMeasure;
// END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS
GetPIDController()->SetInputRange(MIN_ANGLE, MAX_ANGLE);//Minimum voltage defines the max angle, max volage defines the min angle
_targetAngle = 0.0;
_onStage2 = false;
_onStage3 = false;
_targetPosition = CUSTOM;
ResetPID();
}
void MAIN_PROG (uint16_t time)
{
static uint16_t ticker = 0;
if ((uint16_t)(ticker+time)== GetTicker())
{
ticker = GetTicker();
//Encoder section
ENC_PROCESSING (&Left);
ENC_PROCESSING (&Right);
Linear.velo_raw = (Left.velo_fb - Right.velo_fb)/ 2.0f;
Smooth_filter (Linear.velo_raw,Linear.velo_filted,0.1);
PID_VELO_RUN.feedback = Linear.velo_filted[0];
PID_VELO_HOLD.feedback = Linear.velo_filted[0];
Linear.posi_raw = (Left.posi_fb - Right.posi_fb)/ 2.0f;
Smooth_filter (Linear.posi_raw,Linear.posi_filted,0.1);
PID_POSI.feedback = Linear.posi_filted[0];
/*MPU 1*/
/* Read all data */
MPU6050_Status = TM_MPU6050_ReadAll(&MPU6050_Data0);
//Pitch
/* Complementary filter*/
Pitch.raw = (0.98f * (Pitch.raw - MPU6050_Data0.Gyroscope_Y * dt)) + (0.02f * (atan2f(MPU6050_Data0.Accelerometer_X, MPU6050_Data0.Accelerometer_Z) * 180.0f/PI));
Smooth_filter (Pitch.raw,Pitch.filted_theta,0.25);
PID_TILT.feedback = Pitch.filted_theta[0] - Pitch.theta_offset;
//Yaw
// Yaw.raw = - MPU6050_Data0.Gyroscope_Z;
// /* HighPass filter*/
// Yaw.filted_theta[0] = HPF(Yaw.raw,10,f_s);
// PID_TURN.feedback = Yaw.filted_theta[0] - Yaw.theta_offset;
if ((PID_TILT.feedback < 45) && (PID_TILT.feedback > -45))
{
//Preprocessing
if ((setspeed-PID_VELO_RUN.set_point) > 0)
{
PID_VELO_RUN.set_point += 0.5f;
}
if ((setspeed-PID_VELO_RUN.set_point) < 0)
{
PID_VELO_RUN.set_point -= 0.5f;
}
if (PID_VELO_RUN.set_point == 0)
{
//PID POSITION
PID (&PID_POSI,0.34);
PID_VELO_HOLD.set_point = PID_POSI.filted_U[0] * MAX_VELO;
//PID VELOCITY
PID (&PID_VELO_HOLD,0.34);
PID_TILT.set_point = -PID_VELO_HOLD.filted_U[0] * MAX_TILT;
}
else
{
//PID VELOCITY
PID (&PID_VELO_RUN,0.34);
PID_TILT.set_point = -PID_VELO_RUN.filted_U[0] * MAX_TILT;
}
//PID TILTING
PID (&PID_TILT,0.36);
//Turning
//PID (&PID_TURN,0.12);
U_offset = -setturn*TURN_OFFSET;
//FUSION
Left_motor.PWM = PID_TILT.filted_U[0] + U_offset;
Right_motor.PWM = PID_TILT.filted_U[0] - U_offset;
//PWM section
PWM (&Left_motor);
PWM (&Right_motor);
}
else
{
TIM_SetCompare1(TIM4,0);
TIM_SetCompare2(TIM4,0);
TIM_SetCompare3(TIM4,0);
TIM_SetCompare4(TIM4,0);
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
GPIO_ResetBits(GPIOD,GPIO_Pin_13);
GPIO_ResetBits(GPIOD,GPIO_Pin_14);
GPIO_ResetBits(GPIOD,GPIO_Pin_15);
ResetPID (&PID_TILT);
ResetPID (&PID_VELO_HOLD);
ResetPID (&PID_VELO_RUN);
ResetPID (&PID_POSI);
}
}
}
void ShooterArm::SetAngle(float tgtAngle)
{
_targetPosition = CUSTOM;
ResetPID();
SetTargetAngle(tgtAngle);
}
void ShooterArm::SetTargetPosition(ShooterArm::ShooterArmPosition position)
{
static float prevAngle = 180.0;
if (_targetPosition != position)
{
float angle = 0.0;
ResetPID();
_targetPosition = position;
switch (_targetPosition)
{
case ShooterArm::LOAD:
_stage_1_I = 0.04;
_stage_1_D = 0.5;
_stage_2_tolerance = 6;
angle = SHOOTER_ARM_TARGET_LOAD_POSITION;
break;
case ShooterArm::EJECT:
_stage_2_tolerance = 3;
angle = (SHOOTER_ARM_TARGET_EJECT_POSITION);
break;
case ShooterArm::LONG_GOAL:
_stage_2_tolerance = 0.5;
angle = (ARM_TARGET_LONG_GOAL);
Robot::camera->SetIdealRange(7.0, 9.0);
break;
case ShooterArm::SHORT_GOAL:
_stage_2_tolerance = 0.5;
angle = (ARM_TARGET_SHORT_GOAL);
break;
case ShooterArm::REVERSE_SHORT_GOAL:
_stage_2_tolerance = 1;
angle = (ARM_TARGET_REVERSE_SHORT);
break;
case ShooterArm::TRUSS:
_stage_2_tolerance = 3;
angle = (SHOOTER_ARM_TARGET_TRUSS);
break;
case ShooterArm::LONG_TRUSS:
_stage_2_tolerance = 3;
angle = (SHOOTER_ARM_TARGET_LONG_TRUSS);
break;
case ShooterArm::CATCH:
_stage_2_tolerance = 6;
angle = (SHOOTER_ARM_TARGET_CATCH);
break;
case ShooterArm::AUTONOMOUS_1:
_stage_2_tolerance = 1.0;
angle = (ARM_TARGET_AUTONOMOUS_1);
break;
case ShooterArm::AUTONOMOUS_2:
_stage_2_tolerance = 1.0;
angle = (ARM_TARGET_AUTONOMOUS_2);
break;
case ShooterArm::AUTONOMOUS_3:
_stage_2_tolerance = 1.0;
angle = (ARM_TARGET_AUTONOMOUS_3);
break;
case ShooterArm::AUTONOMOUS_CLOSE:
_stage_2_tolerance = 1.0;
angle = (ARM_TARGET_AUTONOMOUS_CLOSE);
break;
default:
angle = (ARM_TARGET_LONG_GOAL);
break;
}
if ((prevAngle < angle) && (angle < 90.0))
{
_stage_2_D = 1.5;
_bothStage_P = 1.1;
}
SetTargetAngle(angle);
prevAngle = angle;
}
}
void StartCalibration(void)
{
ResetPID();
if (CalibrationCycles == 0)
CalibrationCycles = CALIB_CYCLES;
}
