/**
* 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;
}
///////////////////////////////////////////////////////////////////////////////
//////////////////////////////// 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();
//}
}
}
int main(int argc, char **argv)
{
//Register signal and signal handler
signal(SIGINT, signal_callback_handler);
//Init UDP with callbacks and pointer to run status
initUDP( &UDP_Command_Handler, &UDP_Control_Handler, &Running );
print("Eddie starting...\r\n");
initIdentity();
double EncoderPos[2] = {0};
initEncoders( 183, 46, 45, 44 );
print("Encoders activated.\r\n");
imuinit();
print("IMU Started.\r\n");
float kalmanAngle;
InitKalman();
#ifndef DISABLE_MOTORS
print( "Starting motor driver (and resetting wireless) please be patient..\r\n" );
if ( motor_driver_enable() < 1 )
{
print("Startup Failed; Error starting motor driver.\r\n");
motor_driver_disable();
return -1;
}
print("Motor Driver Started.\r\n");
#endif
print("Eddie is starting the UDP network thread..\r\n");
pthread_create( &udplistenerThread, NULL, &udplistener_Thread, NULL );
print( "Eddie is Starting PID controllers\r\n" );
/*Set default PID values and init pitchPID controllers*/
pidP_P_GAIN = PIDP_P_GAIN; pidP_I_GAIN = PIDP_I_GAIN; pidP_D_GAIN = PIDP_D_GAIN; pidP_I_LIMIT = PIDP_I_LIMIT; pidP_EMA_SAMPLES = PIDP_EMA_SAMPLES;
PIDinit( &pitchPID[0], &pidP_P_GAIN, &pidP_I_GAIN, &pidP_D_GAIN, &pidP_I_LIMIT, &pidP_EMA_SAMPLES );
PIDinit( &pitchPID[1], &pidP_P_GAIN, &pidP_I_GAIN, &pidP_D_GAIN, &pidP_I_LIMIT, &pidP_EMA_SAMPLES );
/*Set default values and init speedPID controllers*/
pidS_P_GAIN = PIDS_P_GAIN; pidS_I_GAIN = PIDS_I_GAIN; pidS_D_GAIN = PIDS_D_GAIN; pidS_I_LIMIT = PIDS_I_LIMIT; pidS_EMA_SAMPLES = PIDS_EMA_SAMPLES;
PIDinit( &speedPID[0], &pidS_P_GAIN, &pidS_I_GAIN, &pidS_D_GAIN, &pidS_I_LIMIT, &pidS_EMA_SAMPLES );
PIDinit( &speedPID[1], &pidS_P_GAIN, &pidS_I_GAIN, &pidS_D_GAIN, &pidS_I_LIMIT, &pidS_EMA_SAMPLES );
//Get estimate of starting angle and specify complementary filter and kalman filter start angles
getOrientation();
kalmanAngle = filteredPitch = i2cPitch;
setkalmanangle( filteredPitch );
filteredRoll = i2cRoll;
print( "Eddie startup complete. Hold me upright to begin\r\n" );
double gy_scale = 0.01;
last_PID_ms = last_gy_ms = current_milliseconds();
while(Running)
{
GetEncoders( EncoderPos );
if( fabs(GetEncoder()) > 2000 && !inRunAwayState )
{
print( "Help! I'm running and not moving.\r\n");
ResetEncoders();
inRunAwayState=1;
}
/*Read IMU and calculate rough angle estimates*/
getOrientation();
/*Calculate time since last IMU reading and determine gyro scale (dt)*/
gy_scale = ( current_milliseconds() - last_gy_ms ) / 1000.0f;
last_gy_ms = current_milliseconds();
/*Complementary filters to smooth rough pitch and roll estimates*/
filteredPitch = 0.995 * ( filteredPitch + ( gy * gy_scale ) ) + ( 0.005 * i2cPitch );
filteredRoll = 0.98 * ( filteredRoll + ( gx * gy_scale ) ) + ( 0.02 * i2cRoll );
/*Kalman filter for most accurate pitch estimates*/
kalmanAngle = -getkalmanangle(filteredPitch, gy, gy_scale /*dt*/);
/* Monitor angles to determine if Eddie has fallen too far... or if Eddie has been returned upright*/
if ( ( inRunAwayState || ( fabs( kalmanAngle ) > 50 || fabs( filteredRoll ) > 45 ) ) && !inFalloverState )
{
#ifndef DISABLE_MOTORS
motor_driver_standby(1);
#endif
inFalloverState = 1;
print( "Help! I've fallen over and I can't get up =)\r\n");
}
else if ( fabs( kalmanAngle ) < 10 && inFalloverState && fabs( filteredRoll ) < 20 )
{
if ( ++inSteadyState == 100 )
{
inRunAwayState = 0;
inSteadyState = 0;
#ifndef DISABLE_MOTORS
motor_driver_standby(0);
#endif
inFalloverState = 0;
print( "Thank you!\r\n" );
}
}
else
{
inSteadyState = 0;
}
if ( !inFalloverState )
{
/* Drive operations */
smoothedDriveTrim = ( 0.99 * smoothedDriveTrim ) + ( 0.01 * driveTrim );
if( smoothedDriveTrim != 0 )
{
EncoderAddPos(smoothedDriveTrim); //Alter encoder position to generate movement
}
/* Turn operations */
if( turnTrim != 0 )
{
EncoderAddPos2( turnTrim, -turnTrim ); //Alter encoder positions to turn
}
double timenow = current_milliseconds();
speedPIDoutput[0] = PIDUpdate( 0, EncoderPos[0], timenow - last_PID_ms, &speedPID[0] );//Wheel Speed PIDs
speedPIDoutput[1] = PIDUpdate( 0, EncoderPos[1], timenow - last_PID_ms, &speedPID[1] );//Wheel Speed PIDs
pitchPIDoutput[0] = PIDUpdate( speedPIDoutput[0], kalmanAngle, timenow - last_PID_ms, &pitchPID[0] );//Pitch Angle PIDs
pitchPIDoutput[1] = PIDUpdate( speedPIDoutput[1], kalmanAngle, timenow - last_PID_ms, &pitchPID[1] );//Pitch Angle PIDs
last_PID_ms = timenow;
//Limit PID output to +/-100 to match 100% motor throttle
if ( pitchPIDoutput[0] > 100.0 ) pitchPIDoutput[0] = 100.0;
if ( pitchPIDoutput[1] > 100.0 ) pitchPIDoutput[1] = 100.0;
if ( pitchPIDoutput[0] < -100.0 ) pitchPIDoutput[0] = -100.0;
if ( pitchPIDoutput[1] < -100.0 ) pitchPIDoutput[1] = -100.0;
}
else //We are inFalloverState
{
ResetEncoders();
pitchPID[0].accumulatedError = 0;
pitchPID[1].accumulatedError = 0;
speedPID[0].accumulatedError = 0;
speedPID[1].accumulatedError = 0;
driveTrim = 0;
turnTrim = 0;
}
#ifndef DISABLE_MOTORS
set_motor_speed_right( pitchPIDoutput[0] );
set_motor_speed_left( pitchPIDoutput[1] );
#endif
if ( (!inFalloverState || outputto == UDP) && StreamData )
{
print( "PIDout: %0.2f,%0.2f\tcompPitch: %6.2f kalPitch: %6.2f\tPe: %0.3f\tIe: %0.3f\tDe: %0.3f\tPe: %0.3f\tIe: %0.3f\tDe: %0.3f\r\n",
speedPIDoutput[0],
pitchPIDoutput[0],
filteredPitch,
kalmanAngle,
pitchPID[0].error,
pitchPID[0].accumulatedError,
pitchPID[0].differentialError,
speedPID[0].error,
speedPID[0].accumulatedError,
speedPID[0].differentialError
);
}
} //--while(Running)
print( "Eddie is cleaning up...\r\n" );
CloseEncoder();
pthread_join(udplistenerThread, NULL);
print( "UDP Thread Joined..\r\n" );
#ifndef DISABLE_MOTORS
motor_driver_disable();
print( "Motor Driver Disabled..\r\n" );
#endif
print( "Eddie cleanup complete. Good Bye!\r\n" );
return 0;
}
