diff --git a/src/APP/Aufgabe7/ps7/core0/cfg/pid.h b/src/APP/Aufgabe7/ps7/core0/cfg/pid.h index 6b92bda..4c7732f 100644 --- a/src/APP/Aufgabe7/ps7/core0/cfg/pid.h +++ b/src/APP/Aufgabe7/ps7/core0/cfg/pid.h @@ -8,6 +8,14 @@ #ifndef SRC_APP_AUFGABE7_PS7_CORE0_CFG_PID_H_ #define SRC_APP_AUFGABE7_PS7_CORE0_CFG_PID_H_ +#define PID_BOUND 1000 //Restrict PID Value in [-1000,1000] +#define PI 3.14159265 + +int pid_Init(void *pdata); +int16_t pid_Constrain(int16_t value, int16_t lowerBorder, int16_t higherBorder); +int pid_Task(void *pdata); +int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max); + #endif /* SRC_APP_AUFGABE7_PS7_CORE0_CFG_PID_H_ */ diff --git a/src/APP/Aufgabe7/ps7/core0/src/app_hooks.c b/src/APP/Aufgabe7/ps7/core0/src/app_hooks.c index 99f1f25..dacdb96 100644 --- a/src/APP/Aufgabe7/ps7/core0/src/app_hooks.c +++ b/src/APP/Aufgabe7/ps7/core0/src/app_hooks.c @@ -216,6 +216,7 @@ void App_TaskSwHook (void) #if (APP_CFG_PROBE_OS_PLUGIN_EN > 0) && (OS_PROBE_HOOKS_EN > 0) OSProbe_TaskSwHook(); #endif + GT_TaskSw(); } #endif diff --git a/src/APP/Aufgabe7/ps7/core0/src/gt_tasks.c b/src/APP/Aufgabe7/ps7/core0/src/gt_tasks.c index 822eaf5..e5fd367 100644 --- a/src/APP/Aufgabe7/ps7/core0/src/gt_tasks.c +++ b/src/APP/Aufgabe7/ps7/core0/src/gt_tasks.c @@ -2,8 +2,7 @@ * gt_tasks.c * * Created on: Aug 22, 2014 - * Author: matthiasb - * + * Author: matthiasb */ #include "ucos_uartps.h" @@ -12,96 +11,44 @@ #include "ucos_bsp.h" #include "gt_types.h" #include "gt_cfg.h" -#include /* rand */ + #include /* memset */ #include "ttc_timer.h" #include "imu.h" #include "pid.h" -/*-------------------------------Task model internal struct definitions------------------------------- - * Internal task extension struct - */ -typedef struct -{ - CPU_INT32U BCET; - CPU_INT32U WCET; -}GT_WL_CET_T; -/* - * Internal task extension struct - */ -typedef struct -{ - CPU_INT32U Period; - CPU_INT32U Jitter; - CPU_INT32U Distance; - GT_WL_CET_T CET; -}GT_TASK_EXT_T; /*------------------------------------Execution times and periods-------------------------------------*/ /* * All times given in (1/10)ms */ + /* * Task model */ GT_TASK_EXT_T GT_AllTasks[GT_NUM_OF_TASKS] = { -/* P J D BCET WCET */ +/* Period Jitter Distance BCET WCET */ {50, 0, 0, {1, 1}}, {120, 0, 0, {2, 4}}, {50, 0, 0, {3, 5}}, //{50, 0, 0, {3, 5}}, }; -/*------------------------------------Internal call back functions-------------------------------------*/ -/* - * Function to determine task execution time - */ -static CPU_INT32U _GT_GetTaskCet(GT_TASK_T * pThisTask) -{ - /* - * Simple randomized jitter between BCET and WCET - */ - GT_TASK_EXT_T * pTaskExt = (GT_TASK_EXT_T*)pThisTask->Extension; - CPU_INT32U BCET = pTaskExt->CET.BCET; - CPU_INT32U WCET = pTaskExt->CET.WCET; - return (BCET < WCET) ? (WCET - (rand() % (WCET - BCET))) : WCET; -} - -/* - * Function to determine next task activation - * For this model we use a simple PJD model - */ -static CPU_INT32U _GT_GetNextActivation(GT_TASK_T * pThisTask) -{ - GT_TASK_EXT_T * pTaskExt = (GT_TASK_EXT_T*)pThisTask->Extension; - /* - * Calculate next activation based on PJ Model - */ - CPU_INT32S NextDistance = (pTaskExt->Jitter) ? pTaskExt->Period + (rand()%(pTaskExt->Jitter) - (pTaskExt->Jitter/2)) - : pTaskExt->Period; - /* - * Adjust activation to PJD if needed - */ - return ((CPU_INT32S)pTaskExt->Distance > NextDistance) ? pTaskExt->Distance : (CPU_INT32U)NextDistance; -} - -static CPU_INT32U _GT_ComStackFkt(GT_TASK_T * pThisTask){ -} - /* * Main task table, used for GT_Init call */ GT_TASK_T GT_Tasks[GT_NUM_OF_TASKS] = { - /*Type Activation Type Id Prio Next Activation CET Task init Function Task Function TaskArg D WL/Runable Internal External */ - { GT_TASK_EXT , GT_ACT_INT , 0, 32, _GT_GetNextActivation, _GT_GetTaskCet, your-timer-taskinitfunction ,your-timer_Task , NULL, 50, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[0]}, - { GT_TASK_EXT , GT_ACT_INT , 1, 31, _GT_GetNextActivation, _GT_GetTaskCet, your-imu-taskinitfunction ,your-imu_Task , NULL, 50, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[1]}, - { GT_TASK_EXT , GT_ACT_INT , 2, 33, _GT_GetNextActivation, _GT_GetTaskCet, your-pid-taskinitfunction ,your-pid_Task , NULL, 20, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[2]}, + /*Type Activation Type Id Prio init Function Task Function TaskArg WL/Runable Internal External */ + { GT_TASK_EXT , GT_ACT_INT , 0, 32, your-timer-taskinitfunction ,your-timer_Task , NULL, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[0]}, + { GT_TASK_EXT , GT_ACT_INT , 1, 31, your-imu-taskinitfunction ,your-imu_Task , NULL, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[1]}, + { GT_TASK_EXT , GT_ACT_INT , 2, 33, your-pid-taskinitfunction ,your-pid_Task , NULL, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[2]}, //{ GT_TASK_DEF , GT_ACT_INT , 3, 33, _GT_GetNextActivation, _GT_GetTaskCet, NULL ,NULL , NULL, 20, GT_RUNABLE_NULL, GT_INTERNAL_NULL, (void *)>_AllTasks[3]}, }; + void GT_T00_Start(void){ asm volatile("nop"); } @@ -125,10 +72,6 @@ void GT_T02_Stop(void){ } -/* - * start and end hooks - */ - void GT_TaskStartHook(GT_TASK_T *pMyOwnTask) { switch(pMyOwnTask->Id){ case 0: GT_T00_Start(); break; diff --git a/src/APP/Aufgabe7/ps7/core0/src/pid.c b/src/APP/Aufgabe7/ps7/core0/src/pid.c index ca6a61a..ed81333 100644 --- a/src/APP/Aufgabe7/ps7/core0/src/pid.c +++ b/src/APP/Aufgabe7/ps7/core0/src/pid.c @@ -13,6 +13,79 @@ #include #include #include "pid.h" +#include "imu.h" -//insert your pid-code from task 4 +#define KP 300.0 +#define KD 0.0 +#define KI 200.0 +#define SAMPLE_TIME ((float)(5.0/1000.0)) + +extern float imu_current_angle; + +static float targetAngle = 0.0; + +int32_t pidValue=0; +float errorSumAngle=0; +float prevAngle=0; +int cycle = 0; + + +#define ARRAY_SIZE 1000 +int32_t pidArray[ARRAY_SIZE]; +int arrayIndex = 0; + + +/* * * * * * * * calculates a PID value from -1000 to 1000 by using acc and gyro data * * * * * * * * */ +int pid_Task(void* pdata) { + float errorAngle=0; + CPU_SR cpu_sr; + float currentAngle; + cycle++; + if (cycle == 10 * 1000/5) + targetAngle = prevAngle; + + OS_ENTER_CRITICAL(); + currentAngle = imu_current_angle; + OS_EXIT_CRITICAL(); + + errorAngle = currentAngle - targetAngle; //calc the error to the disired angle + errorSumAngle = errorSumAngle + errorAngle; //integrate the error to use it in the PID + errorSumAngle = (float)CONSTRAIN((int)errorSumAngle, -750, 750); //constrain maximum integrated error + + pidValue = (int16_t)(KP*(errorAngle) + KI*(errorSumAngle)*SAMPLE_TIME + KD/SAMPLE_TIME*(currentAngle-prevAngle)); //calculate output from P, I and D values + pidValue = CONSTRAIN(pidValue, -PID_BOUND, PID_BOUND); //constrain maximum integrated error, to use the max value in calculations for step frequency + + if(pidValue == 0){ //prevent divison by 0 + pidValue=1; + } + + prevAngle = currentAngle; //prepare next iteration + + pidArray[arrayIndex] = pidValue; + arrayIndex = (arrayIndex + 1) % ARRAY_SIZE; + + return 0; +} + +/* * * * * * * * clears Gyro offset and sets it new * * * * * * * * */ +int pid_Init(void *pdata){ + + return 0; +} + +/* * * * * * * * sets outer bounds * * * * * * * * */ +int16_t pid_Constrain(int16_t value, int16_t lowerBorder, int16_t higherBorder){ + if (value > higherBorder){ + value = higherBorder; + } + if (value < lowerBorder){ + value = lowerBorder; + } + return value; +} + +/* * * * * * maps to another numberroom * * * * * * */ +int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) { + return ((x - in_min) * (out_max - out_min)) / (in_max - in_min) + out_min; +}