void ControlInit(void) {
control.reset = 1;
control.status_bits = 0;
control.speeds.angular_speed = 0,
control.speeds.linear_speed = 0;
control.last_finished_id = 0;
control.max_acc = ACC_MAX;
control.max_spd = SPD_MAX;
control.rot_spd_ratio = RATIO_ROT_SPD_MAX;
MotorsInit();
RobotStateInit();
FifoInit();
PIDInit(&PID_left);
PIDInit(&PID_right);
PIDSet(&PID_left, LEFT_P, LEFT_I, LEFT_D, LEFT_BIAS);
PIDSet(&PID_right, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_BIAS);
}
///////////////////////////////////////////////////////////////////////////////
//////////////////////////////// MAIN ///////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
int main(void)
{
IntMasterDisable();
//=====SYSTEM PERIPHERAL INITIALIZATION=====
// Initialize real time clock
mRTCInit(RTC_RATE_HZ);
// Millisecond timekeeping variable
int time;
//Add periodic tasks to be executed by systick
mRTCAddTask(heightSample, HEIGHT_SAMPLE_RATE_HZ);
// Set up display
mDisplayInit(1000000);
mDisplayLine(" Waiting... ", 0, 5, 15);
// Set up buttons
ButtonInit();
// Set up PWM
mPWMInit();
mPWMEnable(PWM4, false); // tail rotor, yaw control.
mPWMEnable(PWM1, false); // main rotor, height control.
//=========CONTROL INITIALIZATION========
//-----altitude PID control------
// Initialize altitude module
heightInit();
// =========PID parameters========================
//Phils
float H_kp = 1;float H_ki = 1.5;float H_kd = -0.5;
short heightPIDOffset = 40;
float Y_kp = 0.6;float Y_ki = 0;float Y_kd = 0;
short YawPIDOffset = 0;//50;
// Windup regulator
float H_windup_limit = 10;
if (H_ki)
H_windup_limit /= H_ki;
// height PID controller
pid_t heightPID;
PIDInit(&heightPID);
PIDSet(&heightPID, H_kp, H_ki, H_kd, H_windup_limit); // set PID constants
// height PID control variables
//35; // PID out offset such that the rotor is at near-takoff speed
short height = 0;
short heightSetpoint = 0; // in degrees
short heightError = 0;
short heightPIDOut = 0;
//-----yaw PID control-------
// Initialise Yaw decoder module
yawInit(); // yaw monitor
float Y_windup_limit = 20; // Maximum integral contribution to PID output value
if (Y_ki)
Y_windup_limit /= Y_ki; // devide by Y_ki to find maximum value in terms of error
// Yaw PID controller
pid_t yawPID;
PIDInit(&yawPID);
PIDSet(&yawPID, Y_kp, Y_ki, Y_kd, Y_windup_limit); // set PID constants
// yaw PID control variables
short yaw = 0;
short yawSetpoint = 0;
short yawError = 0;
short yawPIDOut = 0;
//
// Enable interrupts to the processor.
IntMasterEnable();
short takeOffFlag = false;
while (!takeOffFlag)
{
if (ButtonCheck(SELECT))
//if (GPIOPinRead(GPIO_PORTG_BASE, 0x80))
{
takeOffFlag = true;
}
}
// Reset setpoints to current position
heightSetpoint = heightGet();
yawSetpoint = yawGetAngle();
mPWMEnable(PWM4, true); // tail rotor, yaw control.
mPWMEnable(PWM1, true); // main rotor, height control.
//spin up routine
//spinUp(heightPIDOffset, yawPID);
// Reset clock to zero for effective helicopter launch time
mRTCSet(0);
while (1)
{
//mDisplayClear();
time = mRTCGetMilliSeconds();
// Update Setpoints
updateSetpoints(&yawSetpoint, &heightSetpoint);
// ==================PID Control=================
if ((time % (RTC_RATE_HZ/PID_RATE_HZ)) /*1000/(float)PID_RATE_HZ*/ == 0)
{
//
// ~~~~~~~~~~~~~~~~~ HEIGHT PID ~~~~~~~~~~~~~~~~
height = heightGet();
heightError = heightSetpoint - height;
heightPIDOut = PIDUpdate(&heightPID, heightError, 1.00 / (float)PID_RATE_HZ) + heightPIDOffset;
if (heightPIDOut > 79)
//heightPIDOut = 79;
if (heightPIDOut < 2)
heightPIDOut = 2;
mPWMSet(PWM1, (unsigned short)heightPIDOut);
//
// ~~~~~~~~~~~~~~~~~~ YAW PID ~~~~~~~~~~~~~~~~~~~
yaw = yawGetAngle();
yawError = yaw - yawSetpoint;
yawPIDOut = PIDUpdate(&yawPID, yawError, 1.00 / (float)PID_RATE_HZ) + YawPIDOffset;
if (yawPIDOut > 79)
yawPIDOut = 79;
if (yawPIDOut < 2)
yawPIDOut = 2;
mPWMSet(PWM4, (unsigned short)yawPIDOut);
// ===============================================
}
// RTC_RATE_HZ - PID_RATE_HZ
if (( (time) % 10) == 0)
{
mDisplayLine("time:%.6d mS", time, 0, 7);
mDisplayLine("Yaw = %.4d' ", (int)yaw, 1, 15);
mDisplayLine("YSet = %.4d' ", (int)yawSetpoint, 2, 15);
mDisplayLine("YErr = %.4d' ", (int)yawError, 3, 15);
mDisplayLine("YOut = %.4d' ", (int)yawPIDOut, 4, 15);
mDisplayLine("height = ~%.3d ", (int)height, 5, 15);
mDisplayLine("Hset = %.4d ", (int)heightSetpoint, 6, 15);
mDisplayLine("Herr = %.4d ", (int)heightError, 7, 15);
mDisplayLine("Hout = %.4d ", (int)heightPIDOut, 8, 15);
}
// should put this as part of the main while loop condition
//if (ButtonCheck(SELECT))
//{
// spinDown();
//}
}
}
