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(); //} } }