/** Checkout the most recently acquired FTE
The user will typically call FT_CheckOutFrame() and perform some
apllication specific processing on the frames and then once finished,
the user will then check that FrameTableEntry back in so that it can
be used again.
\return Returns the most recently completed frame. Returns NULL if no new frame is available.
*/
FrameTableEntry* FT_CheckOutFrame()
{
FrameTableEntry* fte = NULL;
CPU_MSR msr = DISABLE_INTERRUPTS();
{
if(g_current_fte->status == FT_STATUS_AVAILABLE)
{
fte = g_current_fte;
fte->status = FT_STATUS_INUSE;
}
//Calculate FPS of Software Processing System
{
static uint32 msLastFrame = 0;
if(fte != NULL)
{
uint32 time = rduint(U_MS);
uint32 diff = time - msLastFrame;
msLastFrame = time;
if(diff != 0) wruint(U_FPS, 1000/diff );
else wruint(U_FPS, 0);
}
}
}
RESTORE_INTERRUPTS(msr);
return fte;
}
/** Checkin an FTE that was checked out for processing
The user will typically call FT_CheckOutFrame() and perform some
apllication specific processing on the frames and then once finished,
the user will then check that FrameTableEntry back in so that it can
be used again.
Checking in an FTE will, by default, CheckIn all the Buffers that the
FTE owns back into the BufferStore. This will cause the FTE to loose
some of its information. So, if there is something that you want to save
then you have better set a static_buffer_option or place it in some other
field in the FTE or else it might get lost.
The static_buffer_option is a bit vector where a 1 indicates that a Buffer
in the frame_address array should not be checked back in by default. Be
careful how this is used. If you for get to checkin some types of buffers
then the BufferStore will run out of them and then new frames will not be
able to check them out.
*/
void FT_CheckInFrame(FrameTableEntry* fte)
{
CPU_MSR msr = DISABLE_INTERRUPTS();
{
if(fte!= NULL)
{
int i;
uint32 static_buffer_options;
fte->status = FT_STATUS_COMPLETED;
// Check in the Buffers that this FTE had checked out
// But do not check in the Static Buffers
static_buffer_options = rdhex(X_STATIC_BUFFER_OPTIONS);
for(i = 0 ; i < VISION_MAX_NUM_FRAMES ; i++)
{
uint32 mask = 0x01 << i;
if(!(static_buffer_options & mask))
BSCheckIn(&fte->frame_address[i]);
}
}
}
RESTORE_INTERRUPTS(msr);
}
inline int Gyro_GetTotalAngle(){
CPU_MSR msr = DISABLE_INTERRUPTS();
int millidegrees = raw_gyro_data.total_angle;
//raw_gyro_data.total_angle = 0;
RESTORE_INTERRUPTS(msr);
return millidegrees * 8.75;
}
/* rtos_init can be a macro in case that USE_DEFAULT_IDLE == 1
* so we undef it here.
*/
void rtos_init(void (*idle)(void*),uint16_t stack_len,uint16_t system ){
set_sleep_mode(SLEEP_MODE_IDLE);
#if USE_DYNAMIC_MEMORY
uint8_t interrupt = GET_INTERRUPTS;
cli();
kernel.system_stack = ( uint8_t * )malloc( sizeof(uint8_t)*system ) + system - 1;
RESTORE_INTERRUPTS(interrupt);
#else
kernel.system_stack = get_stack( sizeof(uint8_t)*system ) + system - 1;
#endif
if ( kernel.system_stack == NULL ){
/**
* If we don't have enough stack for the system,
* we try to warn the user
*/
#if TEST_STACK_OVERFLOW
cli();
ACTION_IN_STACK_OVERFLOW;
while (1);
#endif
return;
}
add_task(idle,NULL, NULL,0,0,0,stack_len);
__asm__ volatile ("rjmp switch_task\n" ::);
};
void Gyro_Calculation(Gyro * gyro)
{
short velocity;
short angular_velocity;
CPU_MSR msr;
//gyro.velocityBack = pid.currentVelocityBack;
msr = DISABLE_INTERRUPTS();
velocity = raw_gyro_data.velocity;
angular_velocity = raw_gyro_data.angular_velocity;
RESTORE_INTERRUPTS(msr);
// V_b
gyro->backVelocity = raw_gyro_data.velocity;
// W
gyro->omega = (CONVERT_TO_RAD_SEC * raw_gyro_data.angular_velocity);
//R_b
gyro->backRadius = gyro->backVelocity/gyro->omega;
// K_b
gyro->backCurvature = gyro->omega/gyro->backVelocity;
// V_f
gyro->frontVelocity = gyro->backVelocity * sqrt(1+((gyro->backCurvature)*gyro->backCurvature*(gyro->wheelBase)*gyro->wheelBase));
// K_f
gyro->frontCurvature = gyro->omega/gyro->frontVelocity;
//R_b
gyro->frontRadius = gyro->frontVelocity/gyro->omega;
// Delta
gyro->steeringAngle = asin(gyro->wheelBase * (1/gyro->frontRadius));
}
/**
This handler is only called when the frame grabber hardware core has finnished reading in a frame to memory
The InterruptHandlerFrameTable() is the interrupt handler that is called
each time the camera core has finished captureing a frame from the camera.
STEPS:
-# Calculate FPS_CAM
-# Update status of recently completed frame
-# Read out the ending frame pointer address and store them in the FrameTableEntry
-# Check in any buffers ownned by the old Entry if it was missed
-# Initiate the next frame capture
*/
void FT_InterruptHandlerFrameTable()
{
static uint32 msLastFrame = 0;
CPU_MSR msr;
msr = DISABLE_INTERRUPTS();
{
wruint(U_FRAME_COUNT, rduint(U_FRAME_COUNT)+1);
//Step: Calculate FPS_CAM
{
uint32 now = rduint(U_MS);
uint32 diff = now - msLastFrame;
msLastFrame = now;
if(diff != 0) wruint(U_FPS_CAM, 1000/diff );
else wruint(U_FPS_CAM, 0);
}
// STEP: Update status of recently completed frame
{
g_capture_fte->status = FT_STATUS_AVAILABLE;
g_capture_fte->frameCount = rduint(U_FRAME_COUNT);
g_capture_fte->camera = &camera0;
g_capture_fte->saveoptions = GetFrameSaveOptions();
}
// STEP: Read out the ending frame pointer address and store them in the FrameTableEntry
// This is important in the accurate determination of the length of list data
// produced by the Camera Core
{
int i;
for(i = 0 ; i < VISION_MAX_NUM_FRAMES ; i++)
g_capture_fte->frame_address_end[i] = (uint8*) GetFrameAddress(i);
}
// STEP: Check in any buffers ownned by the old Entry if it was missed
if((g_current_fte != NULL) && (g_current_fte->status == FT_STATUS_AVAILABLE))
{
FT_CheckInFrame(g_current_fte);
g_current_fte->status = FT_STATUS_MISSED;
}
// STEP: Initiate the next frame capture
g_current_fte = g_capture_fte;
FT_StartCapture(g_capture_fte->next);
}
RESTORE_INTERRUPTS(msr);
}
int Wireless_Send(Skaro_Wireless * w, char command, char length, char * data){
unsigned char top;
CPU_MSR msr = DISABLE_INTERRUPTS();
QueuePush(&(w->write_queue), (void *)(uint32)command);
QueuePush(&(w->write_queue),(void *)(uint32)length);
int i;
for(i = 0; i < length; i++){
QueuePush(&w->write_queue, (void *)(uint32)data[i]);
}
// If there is no send in progress, initiate a send
if (!(w->send_in_progress)) {
// We are about to send it, so pop it off
top = (unsigned char)(uint32)QueuePop(&(w->write_queue));
w->send_in_progress = 1;
XUartLite_Send(&(w->uart), &top, 1);
}
RESTORE_INTERRUPTS(msr);
return 1;
}
/* innitialize Timer RC */
void TimerRC_Init(void)
{
DISABLE_INTERRUPTS(); // non-interruptus
msttrc = 0; // Timer RC off standby
trccnt = 0;
trcgra = (USHORT)(TIMERRC_DIVISOR);
trccr1 = 0x80 | TIMERRC_CLK_SRC; // trccnt cleared by trcgra compare match, select timer clock source
trcior0 = 0x88; // IOB = output compare, output disabled, IOA = output compare, output disabled
trcior1 = 0x00; // IOD = output compare, output disabled, IOC = output compare, output disabled
ilvl34 = 1; // set TRC interrupt to priority level 01
ilvl35 = 0;
imiea_trcier = 1; // enable the interrupt
cts_trcmr = 1; // start the timer
RESTORE_INTERRUPTS(); // interruptus-maximus
}
/** Print out debug information to the GUI
*/
void FT_PrintFrameTable()
{
CPU_MSR msr;
xil_printf("FT_PrintFrameTable()\r\n");
msr = DISABLE_INTERRUPTS();
{
int i,j;
for(i = 0 ; i < NUM_FRAME_TABLE_ENTRIES ; i++)
{
xil_printf("FT_PrintFrameTable(): i:\t%d\tloc: 0x%x\r\n", i, &g_frametable[i]);
xil_printf("FT_PrintFrameTable(): status:\t%d\r\n", g_frametable[i]->status);
for(j = 0 ; j < VISION_MAX_NUM_FRAMES ; j++)
{
if(j != VISION_FRAME_NOT_AVAILABLE)
xil_printf("FT_PrintFrameTable(): Frame %d :\t0x%x\r\n", j,(uint32)g_frametable[i]->frame_address[j]);
}
}
}
RESTORE_INTERRUPTS(msr);
}
/* initialize Timer RJ2 */
void TimerRJ2_Init(void)
{
DISABLE_INTERRUPTS(); // non-interruptus
msttrj = 0; // Timer RJ2 off standby
trj = TIMERRJ2_DIVISOR; // trj freq = TIMERRJ2_CLK_SRC / TIMERRJ2_DIVISOR
trjioc = 0x00; // controls output stuff, which we're not using here.
trjmr = TIMERRJ2_CLK_SRC; // select timer clock source
trjisr = 0x00; // controls output event stuff, which we're not using here
ilvlb0 = 1; // set TRJ interrupt to priority level 01
ilvlb1 = 0;
trjie_trjir = 1; // enable the interrupt
tstart_trjcr = 1; // start the timer
RESTORE_INTERRUPTS(); // restorus interruptus
}
int add_task(void (*f)(void*),void (*finisher)(void), void * init_data,uint16_t period,uint16_t delay, uint8_t priority, uint16_t stack_len ){
task_t * new_task = NULL,* current, *previous;
uint16_t min_stack_len = 0xFFFF;
uint8_t interrupt;
current = previous = kernel.stopped_tasks;
while ( current != NULL )
{
/* Finding an task already created but stopped and which stack we can use*/
if ( current->stack_len >= stack_len && current->stack_len <= min_stack_len )
{
if ( previous == current )
kernel.stopped_tasks = current->next_task;
else
previous->next_task = current->next_task;
new_task = current;
min_stack_len = current->stack_len;
break;
}
previous = current;
current = current->next_task;
}
if ( new_task == NULL )
{
interrupt = GET_INTERRUPTS;
cli();
#if USE_DYNAMIC_MEMORY
new_task = ( task_t * ) malloc(sizeof(task_t));
if ( new_task == NULL )
{
RESTORE_INTERRUPTS(interrupt);
return NO_MEMORY;
}
new_task->bottom_stack = ( uint8_t * )malloc( sizeof(uint8_t)*stack_len );
if ( new_task->bottom_stack == NULL)
{
free(new_task);
RESTORE_INTERRUPTS(interrupt);
return NO_MEMORY;
}
#else
if ( __current_position_tasks >= NUMBER_OF_TASKS )
{
RESTORE_INTERRUPTS(interrupt);
return NO_MEMORY;
}
new_task = &__all_taks[__current_position_tasks++];
new_task->bottom_stack = get_stack( sizeof(uint8_t)*stack_len );
if ( new_task->bottom_stack == NULL)
{
RESTORE_INTERRUPTS(interrupt);
return NO_MEMORY;
}
#endif
RESTORE_INTERRUPTS(interrupt);
new_task->stack_len = stack_len;
}
// This can be used to detect stack overflows.
new_task->stack = new_task->bottom_stack + sizeof(uint8_t)*new_task->stack_len-1;
// To jump start the task.
// we use another function with no arguments.
*((new_task->stack)--) = ((uint16_t)task_starter) & 0xFF;
*((new_task->stack)--) = (((uint16_t)task_starter) >> 8) & 0xFF;
new_task->priority = priority;
new_task->period = period;
new_task->delay = delay;
new_task->init_data = init_data;
#if USE_MUTEX
new_task->holding_mutex = NULL;
#endif
//placing arguments in the stack for further use
*((new_task->stack)--) = ((uint16_t)init_data) & 0xFF;
*((new_task->stack)--) = (((uint16_t)init_data) >> 8) & 0xFF;
new_task->func = f;
new_task->finisher = finisher;
interrupt = GET_INTERRUPTS;
if ( delay == 0 ){
new_task->state = TASK_STARTING;
add_task_to_priority_list(new_task, &kernel.ready_tasks);
}
else{
new_task->state = TASK_DELAYED;
add_task_to_blocked(new_task, &kernel.spleeping_tasks);
}
RESTORE_INTERRUPTS(interrupt);
return 0;
}
void PID_UpdateRadius(PID * pid)
{
int P, I, D;
double refreshRate;
CPU_MSR msr;
//------Read Encoder and clock
msr = DISABLE_INTERRUPTS();
float desiredRadius = pid->desiredRadiusPID;//----//get info from camera here
uint32 nowClocks = ClockTime();
// Gyro_Calculation(pid->gyro);
RESTORE_INTERRUPTS(msr);
//------Time since last function call in seconds
refreshRate = refresh_rate(nowClocks, pid->lastClockTicks_r);
pid->lastClockTicks_r = nowClocks;
//------Current Radius at front of car
pid->currentRadius = pid->gyro->frontRadius;
if (pid->currentRadius > 6000)
pid->currentRadius = 6000;
else if (pid->currentRadius < -6000)
pid->currentRadius = -6000;
//------Calculate Error
pid->error_r = ((float)(pid->currentRadius - desiredRadius))/(pid->currentRadius * desiredRadius);
//------Update Derivative
pid->differentiator_r = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_r) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentRadius - pid->lastCurrentRadius));
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if (abs(((int)(1000000*pid->error_r))) > 200)
pid->integrator_r = ((int)((pid->integrator_r))) + ((int)(1000000*((refreshRate/2)*(pid->error_r + pid->lastError_r))));
//------Output Calculation
//pid->Ki_r = pid->Ki_r/1000000;
P = (pid->Kp_r * pid->error_r);
I = (pid->Ki_r * pid->integrator_r);
D = (pid->Kd_r * pid->differentiator_r);
pid->radiusEqualibrium = (40000.0/desiredRadius);
// Wireless_Debug("P: ");
// PrintInt(P);
// Wireless_Debug("\n\r");
// Wireless_Debug("Integral: ");
// PrintFloat(pid->integrator_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("error: ");
// PrintFloat(pid->error_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("lastError: ");
// PrintFloat(pid->lastError_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("Refresh: ");
// PrintFloat(refreshRate);
// Wireless_Debug("\n\r");
// Wireless_Debug("I: ");
// PrintInt(I);
// Wireless_Debug("\n\r");
// Wireless_Debug("velocityBack: ");
// PrintFloat(pid->currentVelocityBack);
// Wireless_Debug("\n\r");
// Wireless_Debug("velocityFront: ");
// PrintFloat(pid->currentVelocity);
// Wireless_Debug("\n\r");
// Wireless_Debug("-----------------------------");
// Wireless_Debug("\n\r");
// Wireless_Debug("D: ");
// PrintInt(D);
// Wireless_Debug("\n\r");
// Wireless_ControlLog_Ext(pid->currentRadius, pid->desiredRadiusPID, pid->error, pid->outputPID_unsat, P);
//pid->outputPID_unsat_r = (P + I - D) + pid->radiusEqualibrium;
pid->outputPID_unsat_r = pid->radiusEqualibrium;
pid->outputPID_r = sat(pid->outputPID_unsat_r, 40);
//pid->integrator_r = ((int)pid->integrator_r) + ((int)(1000000*(refreshRate/pid->Ki_r)*(pid->outputPID_r - pid->outputPID_unsat_r)));
//------Save Info for graph
// Wireless_ControlLog_Ext(pid->currentRadius, desiredRadius, pid->error_r, pid->outputPID_unsat_r, I);
//------Save states and send PWM to motors
pid->lastCurrentRadius = pid->currentRadius;
pid->lastError_r = pid->error_r;
pid->lastIntegrator_r = pid->integrator_r;
// Wireless_Debug("Last Error: ");
// PrintFloat(pid->lastError_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("Integral: ");
// PrintFloat(pid->integrator_r);
// Wireless_Debug("\n\r");
// Wireless_Debug("-----------------------------");
// Wireless_Debug("\n\r");
SetServo(RC_STR_SERVO, pid->outputPID_r);
}
void PID_UpdateVelocity(PID * pid)
{
int desiredVelocity = pid->desiredVelocityPID;
float P, I, D;
float refreshRate;
CPU_MSR msr;
//------Read Encoder and clock
msr = DISABLE_INTERRUPTS();
pid->encoderValue = getTicks();
uint32 nowClocks = ClockTime();
Gyro_Calculation(pid->gyro);
RESTORE_INTERRUPTS(msr);
//------Time since last function call in seconds
refreshRate = refresh_rate(nowClocks, pid->lastClockTicks);
pid->lastClockTicks = nowClocks;
//------Distance since last function call in ticks
int encoderDifference = pid->encoderValue - pid->lastEncoderValue;
//------Calculate Velocity
pid->currentVelocity = (int)((encoderDifference) / (refreshRate));
//pid->currentVelocityBack = (int)((encoderDifference) / (refreshRate));
//------Convert Back Velocity to front
//pid->currentVelocity = Gyro_VelocityBackToFront(pid->gyro,pid->currentVelocityBack);
//------Calculate Error
pid->error = desiredVelocity - (pid->currentVelocity);
//------Update Derivative
//pid->differentiator = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentVelocity - pid->lastCurrentVelocity));
pid->differentiator = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator) + ((2/((2*pid->Tau)+refreshRate))*(pid->error - pid->lastError));
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
//If we have switched directions, zero the integrator
if (abs(pid->lastDesiredVelocity) / pid->lastDesiredVelocity !=
abs(pid->desiredVelocityPID) / pid->desiredVelocityPID) {
pid->integrator = 0;
} else {
pid->integrator = pid->integrator + ((refreshRate/2)*(pid->error + pid->lastError));
}
//------Output Calculation
P = pid->Kp * pid->error;
I = pid->Ki * pid->integrator;
D = pid->Kd * pid->differentiator;
pid->outputPID_unsat = ((int)P) + ((int)I) - ((int)D);
pid->outputPID = sat(pid->outputPID_unsat, 40);
//pid->outputPID = downShift(pid->error, -600, 7);
pid->integrator = pid->integrator + (refreshRate/pid->Ki)*(pid->outputPID - pid->outputPID_unsat);
//Wireless_ControlLog_Ext(pid->currentVelocity, desiredVelocity, pid->error, pid->outputPID_unsat, P);
//------Save states and send PWM to motors
pid->lastEncoderValue = pid->encoderValue;
pid->lastCurrentVelocity = pid->currentVelocity;
pid->lastDesiredVelocity = desiredVelocity;
SetServo(RC_VEL_SERVO, (int)pid->outputPID);
}
void updateVelocityOutput()
{
float desiredVelocity = pid.desiredVelocityPID;
float P, I, D, currentVelocity;
uint32 deltaClocks;
CPU_MSR msr;
//------Read Encoder and clock
msr = DISABLE_INTERRUPTS();
pid.encoderValue = getTicks();
uint32 nowClocks = ClockTime();
RESTORE_INTERRUPTS(msr);
//------Time since last function call in seconds
uint32 maxClocks = 0xffffffff;
if ((nowClocks < pid.lastClockTicks))
deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks;
else
deltaClocks = nowClocks - pid.lastClockTicks;
float refreshRate = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec
//uint32 refreshRate = (deltaClocks)/(XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ/1000) ;
//xil_printf("nowClock: %d\r\n", nowClocks);
//xil_printf("pid.lastClockTicks: %d\r\n", pid.lastClockTicks);
//xil_printf("Delta Clock: %d\r\n", deltaClocks);
//xil_printf("Refresh Rate: ");
//PrintFloat(refreshRate);
//print("\n\r");
pid.lastClockTicks = nowClocks;
//------Distance since last function call in ticks
int encoderDifference = pid.encoderValue - pid.lastEncoderValue;
//xil_printf("Encoder Value: %d\r\n", pid.encoderValue);
//xil_printf("Last Encoder Value: %d\r\n", pid.lastEncoderValue);
//xil_printf("Encoder Diff: %d\r\n", encoderDifference);
//------Calculate velocity
currentVelocity = ((encoderDifference) / (refreshRate));
//xil_printf("Current Velocity: ");
PrintFloat(currentVelocity);
xil_printf(",");
//xil_printf("Desired Velocity: ");
PrintFloat(desiredVelocity);
xil_printf(",");
//print("\n\r");
//------Error
pid.error = desiredVelocity - (currentVelocity);
// xil_printf("error: ");
// PrintFloat(pid.error);
// print("\n\r");
//------Update Derivative
pid.differentiator = (2*pid.Tau-refreshRate)/(2*pid.Tau+refreshRate)*pid.differentiator + 2/(2*pid.Tau+refreshRate)*(pid.error-pid.lastError);
//xil_printf("differentiator: ");
//PrintFloat(pid.differentiator);
//print("\n\r");
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if ((pid.error < 1500 && pid.error > -1500) && (pid.error > 10 || pid.error < -10)){
pid.integrator = pid.integrator + (refreshRate/2)*(pid.error + pid.lastError);
} else {
pid.integrator = 0;
}
// if(pid.error == 0) {
// pid.integrator = 0;
// }
// xil_printf("integrator: ");
// PrintFloat(pid.integrator);
// print("\n\r");
//------Output Calculation
P = pid.Kp * pid.error;
I = pid.Ki * pid.integrator;
D = pid.Kd * 0;//pid.differentiator;
pid.outputPID_unsat = P + I - D;
pid.outputPID = sat(pid.outputPID_unsat, 60);
//------Save states and send PWM to motors
pid.lastEncoderValue = pid.encoderValue;
pid.lastCurrentVelocity = currentVelocity;
pid.lastDesiredVelocity = desiredVelocity;
SetServo(RC_VEL_SERVO, (int)pid.outputPID);
// xil_printf("Output PID unsat: ");
// PrintFloat(pid.outputPID_unsat);
// print("\n\r");
// xil_printf("Output PID: %d", pid.outputPID);
// print("--------------------------------------\r\n");
}
void updateDistanceOutput()
{
int32 desiredDistance = pid.desiredDistancePID;
int32 P, I, D;
uint32 deltaClocks;
CPU_MSR msr;
msr = DISABLE_INTERRUPTS();
pid.encoderValue = getTicks();
uint32 nowClocks = ClockTime();
RESTORE_INTERRUPTS(msr);
//logData(desiredDistance, pid.encoderValue);
//------Time since last function call in seconds
uint32 maxClocks = 0xffffffff;
if ((nowClocks < pid.lastClockTicks))
deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks;
else
deltaClocks = nowClocks - pid.lastClockTicks;
float deltaTime = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec
pid.error_d = desiredDistance - pid.encoderValue;
//
// xil_printf("Encoder Value: %d\r\n", pid.encoderValue);
// xil_printf("Desired Distance: %d\r\n", desiredDistance);
// xil_printf("error_d: %d\r\n", pid.error_d);
//------Update integrator - AntiWindup(only use the integrator if we are close, but not too close)
if ((pid.error_d < 1000 && pid.error_d > -1000) && (pid.error_d > 100 || pid.error_d < -100)){
pid.integrator_d = pid.integrator_d + (deltaTime/2)*(pid.error_d + pid.lastError_d);
}
else
pid.integrator_d = 0;
//------only use the integrator if we are close, but not too close
// if (pid.error_d < 500 && pid.error_d > -500 &&
// (pid.error_d > 100 || pid.error_d < -100))
// pid.integrator_d += pid.error_d * deltaTime;
// else
// pid.integrator_d = 0;
//print("integrator: ");
//PrintFloat(pid.integrator_d);
//print("\n\r");
//------Update Derivative
pid.differentiator_d = (2*pid.Tau-deltaTime)/(2*pid.Tau+deltaTime)*pid.differentiator_d + 2/(2*pid.Tau+deltaTime)*(pid.error_d-pid.lastError_d);
//print("differentiator: ");
//PrintFloat(pid.differentiator_d);
P = pid.Kp_d * pid.error_d;
I = pid.Ki_d * pid.integrator_d;
D = pid.Kd_d * pid.differentiator_d;
//print("P: ");
//PrintFloat(P);
//print("\n\r");
//print("I: ");
//PrintFloat(I);
//print("\n\r");
//print("D: ");
//PrintFloat(D);
//print("\n\r");
pid.outputPID_unsat = P + I - D;
pid.outputPID = sat(pid.outputPID_unsat, 60);
//------if we are really close, don't do anything!
if (pid.error_d < 100 && pid.error_d > -100) {
pid.outputPID = 0;
}
//------Integrator Anti-windup
// if (pid.Ki_d != 0.0f) {
// pid.integrator_d = pid.integrator_d + deltaTime/pid.Ki_d * (pid.outputPID - pid.outputPID_unsat);
// }
// print("integrator_antiWind: ");
// PrintFloat(pid.integrator_d);
//print("\n\r");
pid.lastError_d = pid.error_d;
pid.lastDesiredDistance = desiredDistance;
SetServo(RC_VEL_SERVO, pid.outputPID);
//xil_printf("Output PID: %d\r\n", pid.outputPID);
//print("--------------------------------------\r\n");
}
