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;
}
