void MashinePropery::Mashine_Init(PID *pp) { for (int i = 0; i < 6; i++) { angle[i] = angle_init[i]; goal_angle[i] = angle_init[i]; } #ifdef OLEDPRINT oled.ssd1306_init(SSD1306_SWITCHCAPVCC); oled.clear(); oled.drawstring(0, 0, "Mesh"); oled.drawstring(30, 0, "goal"); oled.drawstring(66, 0, "now"); oled.drawstring(0, 2, "M0"); oled.drawstring(0, 3, "M1"); oled.drawstring(0, 4, "M2"); oled.drawstring(0, 5, "M3"); oled.drawstring(0, 6, "M4"); oled.drawstring(0, 7, "M5"); oled.display(); oledprintgoal(); #endif // OLEDPRINT PIDInit(pp); }
void pidInitPIDObj(pidObj* pid, int Kp, int Ki, int Kd, int Kaw, int Kff) { pid->input = 0; pid->dState = 0; pid->iState = 0; pid->output = 0; pid->y_old = 0; pid->p = 0; pid->i = 0; pid->d = 0; //This is just a guess for the derivative filter time constant pid->N = 5; pid->Kp = Kp; pid->Ki = Ki; pid->Kd = Kd; pid->Kaw = Kaw; pid->Kff = Kff; pid->onoff = PID_OFF; pid->error = 0; #ifdef PID_HARDWARE pidHWSetFracCoeffs(&(pid->dspPID), pid->Kp, pid->Ki, pid->Kd); if((pid->dspPID.abcCoefficients != NULL) && (pid->dspPID.controlHistory != NULL) ){ PIDInit(&(pid->dspPID)); } #endif }
void pidHWCreate(tPID* controller, fractional* coeffs, fractional* hist) { // Set up pointers to the derived coefficents and controller histories controller->abcCoefficients = coeffs; controller->controlHistory = hist; // Initialize the PID controller PIDInit(controller); }
void Init(){ sonarInit(); PIDInit(myWallFollower,0.05,5,0); DriveInit(myWheels); DriveSpeedLinear(myWheels, 10); printf("Init done"); fflush(stdout); }
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 function demonstrating the use of PID(), PIDInit() and PIDCoeffCalc() functions from DSP library in MPLAB C30 v3.00 and higher */ int initPIDs(void) { /* Step 1: Initialize the PID data structure, PID */ PID1.abcCoefficients = &abcCoefficient1[0]; /*Set up pointer to derived coefficients */ PID1.controlHistory = &controlHistory1[0]; /*Set up pointer to controller history samples */ PIDInit(&PID1); /*Clear the controler history and the controller output */ kCoeffs1[0] = Q15(1.0); kCoeffs1[1] = Q15(0.0); kCoeffs1[2] = Q15(0.0); PIDCoeffCalc(&kCoeffs1[0], &PID); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */ PID2.abcCoefficients = &abcCoefficient2[0]; /*Set up pointer to derived coefficients */ PID2.controlHistory = &controlHistory2[0]; /*Set up pointer to controller history samples */ PIDInit(&PID2); /*Clear the controler history and the controller output */ kCoeffs2[0] = Q15(1.0); kCoeffs2[1] = Q15(0.0); kCoeffs2[2] = Q15(0.0); PIDCoeffCalc(&kCoeffs2[0], &PID2); /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */ }
/****************************************************************************** * Function: main(void) * * Output: None * * Overview: Main function used to init the ADC, PWM and TIMER2 modules. * It also inits the global variables used in the interrupts and * PI controller. * The main task executed here is to start and stop the motor * as well as setting the ramp-up initial parameters to * spin the motor * * Note: None *******************************************************************************/ int main(void) { // Configure Oscillator to operate the device at 30Mhz // Fosc= Fin*M/(N1*N2), Fcy=Fosc/2 // Fosc= 7.37*32/(2*2)= 58.96Mhz for 7.37 input clock /****************** Clock definitions *********************************/ PLLFBD=30; // M=32 CLKDIVbits.PLLPOST=0; // N1=2 CLKDIVbits.PLLPRE=0; // N2=2 OSCTUN=0; // Tune FRC oscillator, if FRC is used // Disable Watch Dog Timer RCONbits.SWDTEN=0; // Clock switch to incorporate PLL __builtin_write_OSCCONH(0x01); // Initiate Clock Switch to // FRC with PLL (NOSC=0b001) __builtin_write_OSCCONL(0x01); // Start clock switching while (OSCCONbits.COSC != 0b001); // Wait for Clock switch to occur // Wait for PLL to lock while(OSCCONbits.LOCK!=1) {}; /****************** PID init **************************************/ #ifdef CLOSELOOPMODE ReferenceSpeed = (MIN_DUTY_CYCLE*2)/3; // load the PID gain coeffecients into an array; PIDGainCoefficients[0] = SpeedControl_P; PIDGainCoefficients[1] = SpeedControl_I; PIDGainCoefficients[2] = SpeedControl_D; // Initialize the PIDStructure variable before calculation the K1, K2, and K3 terms PIDStructure.abcCoefficients = abcCoefficients; PIDStructure.controlHistory = controlHistory; PIDCoeffCalc(PIDGainCoefficients, &PIDStructure); // initialize control history PIDInit(&PIDStructure); PIDStructure.controlOutput = MIN_DUTY_CYCLE; /****************************************************************/ #endif /****************** I/O port Init *********************************/ /* Configuring Digital PORTS multiplexed with PWMs as outputs*/ LATBbits.LATB10 = 0; //PWM1H3 TRISBbits.TRISB10 = 0; LATBbits.LATB11 = 0; //PWM1L3 TRISBbits.TRISB11 = 0; LATBbits.LATB12 = 0; //PWM1H2 TRISBbits.TRISB12 = 0; LATBbits.LATB13 = 0; //PWM1L2 TRISBbits.TRISB13 = 0; LATBbits.LATB14 = 0; //PWM1H1 TRISBbits.TRISB14 = 0; LATBbits.LATB15 = 0; //PWM1L1 TRISBbits.TRISB15 = 0; /*Push Buttons ports*/ LATBbits.LATB4 = 0; TRISBbits.TRISB4 = 1; LATAbits.LATA8 = 0; TRISAbits.TRISA8 = 1; /****************** Functions init *********************************/ INTCON1bits.NSTDIS = 0; // Enabling nested interrupts InitMCPWM(); //Configuring MC PWM module InitADC10(); //Configuring ADC InitTMR2(); //Configuring TIMER 3, used to measure speed InitTMR1(); //Configuring TIMER 1, used for the commutation delay Flags.RotorAlignment = 0; // TURN OFF RAMP UP Flags.RunMotor = 0; // indication the run motor condition /****************** Infinite Loop *********************************/ for(;;) { while (!S3); // wait for S3 button to be hit while (S3) // wait till button is released DelayNmSec(DEBOUNCE_DELAY); T2CONbits.TON = 1; // Start TIMER , enabling speed measurement PWM1CON1 = 0x0777; // enable PWM outputs /*ROTOR ALIGNMENT SEQUENCE*/ Flags.RotorAlignment = 1; // TURN ON rotor alignment sequence Flags.RunMotor = 1; // Indicating that motor is running CurrentPWMDutyCycle = MIN_DUTY_CYCLE; //Init PWM values DesiredPWMDutyCycle = MIN_DUTY_CYCLE; //Init PWM values PWMticks = 0; //Init Rotor aligment counter /************* Rotor alignment sequence and ramp-up sequence ************/ for(RampUpCommState=1;RampUpCommState<7;RampUpCommState++) { while(++PWMticks<MAX_PWM_TICKS) P1OVDCON=PWM_STATE[RampUpCommState]; PWMticks = 0; } Flags.RotorAlignment = 0; // TURN OFF rotor alignment sequence PWMticks = MAX_PWM_TICKS+1; // RAMP UP for breaking the motor IDLE state DelayNmSec(RAM_UP_DELAY); // RAMP UP DELAY Sub_timer=Sub_timer++; /****************** Motor is running *********************************/ while(Flags.RunMotor) // while motor is running { /****************** Stop Motor *********************************/ //if (S3) // if S3 is pressed //{ Sub_timer=0; // Musses while ( Sub_timer=Sub_timer++<=600) // Musses //while (S3) // wait for key release DelayNmSec(DEBOUNCE_DELAY); PWM1CON1 = 0x0700; // disable PWM outputs P1OVDCON = 0x0000; // override PWM low. Flags.RotorAlignment = 0; // turn on RAMP UP Flags.RunMotor = 0; // reset run flag CurrentPWMDutyCycle = 1; // Set PWM to the min value //Initi speed measurement variables & timer T2CONbits.TON = 0; // Stop TIMER2 TMR2 = 0; //Clear TIMER2 register Timer2Value = 0; Timer2Average = 0; #ifdef CLOSELOOPMODE //Init PI controller variables DesiredSpeed = (int)((ReferenceSpeed*3)/2); CurrentSpeed = 0; // load the PID gain coeffecients into an array; PIDGainCoefficients[0] = SpeedControl_P; PIDGainCoefficients[1] = SpeedControl_I; PIDGainCoefficients[2] = SpeedControl_D; // Initialize the PIDStructure variable before calculation the K1, K2, and K3 terms PIDStructure.abcCoefficients = abcCoefficients; PIDStructure.controlHistory = controlHistory; PIDCoeffCalc(PIDGainCoefficients, &PIDStructure); // initialize control history PIDInit(&PIDStructure); PIDStructure.controlOutput = MIN_DUTY_CYCLE; #endif // } }//end of motor running loop }//end of infinite loop return 0; }//end of main function
/////////////////////////////////////////////////////////////////////////////// //////////////////////////////// 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(); //} } }
/** * Run all tests */ int umain (void) { isActivated = false; isForward = true; MagnetInit(); PIDInit(); uint8_t num_low_readings = 0; activateRun(); while(1){ //Read Magnet data ComputeMagnetTarget(); //uint8_t oldForward = isForward; if (fangle > 90 || fangle < -90) { isForward = false; } else { isForward = true; } //Determine if we should change from activated->deactivated or vice versa if (isActivated) { if (FieldStrength < DEACTIVATE_THRESH) { num_low_readings++; //Need START_RAMPDOWN readings to begin motor rampdown if (num_low_readings > START_RAMPDOWN) { SetTargetVelocity(TARGET_VELOCITY*(1.0-(float)(num_low_readings-START_RAMPDOWN)/(float)(DEACTIVATE_SAMPLES-START_RAMPDOWN))); //SetTargetVelocity(TARGET_VELOCITY); } //If you have DEACTIVATE_SAMPLES low readings, deactivate if (num_low_readings == DEACTIVATE_SAMPLES) { isActivated = false; num_low_readings = 0; SetTargetVelocity(TARGET_VELOCITY); printf("\nDEACTIVATED"); } } else { //We had a good reading, reset bad reading count and motor velocity num_low_readings = 0; SetTargetVelocity(TARGET_VELOCITY); } } else { //Activate on a single high reading if (FieldStrength > ACTIVATE_THRESH) { isActivated = true; SetTargetVelocity(TARGET_VELOCITY); printf("\nACTIVATED"); PIDReset(); } } SetTargetVelocity(NearbyScale*TargetVelocity); //Stop if not activated, otherwise update PID if (!isActivated) { CoastMotors(); //pause(100); } else { PIDUpdate(); } //pause(100); } return 0; }