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