ejung/sdes_lab
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

moving

Browse Source
This commit is contained in:
sdes1
2026-01-13 19:33:51 +01:00
parent 47f75e5b40
commit 66f32e30c0
3 changed files with 47 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/APP/Aufgabe3/ps7/core0/src/ttc_timer.c
View File

@@ -28,28 +28,31 @@ XTtcPs xttcps[2];
XGpioPs xgpiops; XGpioPs xgpiops;
XGpioPs_Config* gpioCfg; XGpioPs_Config* gpioCfg;
extern int16_t pidValue; extern int32_t pidValue;
void timer_Task(void *pdata) { void timer_Task(void *pdata) {
timer_Init(); while(1)
if(pidValue){
uint16_t interval = 1.337*(64000/abs(pidValue));
uint8_t direction = pidValue<0;
//UCOS_Printf("%d %d\r\n", direction, interval);
//UCOS_Printf("counter value: %d\r\n", XTtcPs_GetCounterValue(&xttcps));
timer_Stop(0);
timer_Stop(1);
timer_Set_Interval_Length(0,interval);
timer_Set_Interval_Length(1,interval);
timer_Start(0);
timer_Start(1);
motor_Set_Moving_Direction(54, direction);
motor_Set_Moving_Direction(55, !direction);
}
else
{ {
timer_Stop(0); if(pidValue){
uint16_t interval = 1.337*(64000/abs(pidValue));
uint8_t direction = pidValue<0;
//UCOS_Printf("%d %d\r\n", direction, interval);
//UCOS_Printf("counter value: %d\r\n", XTtcPs_GetCounterValue(&xttcps));
timer_Stop(0);
timer_Stop(1);
timer_Set_Interval_Length(0,interval);
timer_Set_Interval_Length(1,interval);
timer_Start(0);
timer_Start(1);
motor_Set_Moving_Direction(54, direction);
motor_Set_Moving_Direction(55, !direction);
}
else
{
timer_Stop(0);
timer_Stop(1);
}
OSTimeDly(1);
} }
} }

35
src/APP/Aufgabe4/ps7/core0/src/imu.c
View File

@@ -18,6 +18,7 @@
#include "ucos_ii.h" #include "ucos_ii.h"
#include "ucos_bsp.h" #include "ucos_bsp.h"
#include <math.h>
/** /**
* Global variables * Global variables
*/ */
@@ -33,6 +34,7 @@ int16_t gyroData[3];
int16_t accelBuffer[5000]; int16_t accelBuffer[5000];
int16_t gyroBuffer[5000]; int16_t gyroBuffer[5000];
int16_t angleBuffer[5000];
static XIicPs Iic; static XIicPs Iic;
XIicPs_Config *Config; XIicPs_Config *Config;
@@ -53,28 +55,33 @@ static void mpu9250_Get_Acc_Min_Max();
* Read sensor data and calculate angle * Read sensor data and calculate angle
*/ */
int mpu9250_CalculateAngle(void *pdata){ int mpu9250_CalculateAngle(void *pdata){
static uint16_t i=0;
mpu9250_Read_Data(59, 6, (u8*)accelData);
mpu9250_Read_Data(67, 6, (u8*)gyroData);
int16_t accelX = (int16_t)((accelData[0]&0xFF)<<8 | (accelData[0]&0xFF00)>>8);
int16_t accelZ = (int16_t)((accelData[2]&0xFF)<<8 | (accelData[2]&0xFF00)>>8);
double accelAngle = atan2(accelX,accelZ)*180/PI;
double gyroAngleD = (int16_t)((gyroData[1]&0xFF)<<8 | (gyroData[1]&0xFF00)>>8);
accelBuffer[i] = accelAngle;
gyroBuffer[i] = gyroAngleD;
imu_current_angle = 0.98*(imu_current_angle - (0.012*250*gyroAngleD/INT16_MAX)) + 0.02*accelAngle;
//current_angle = current_angle - gyroBuffer[i];
//UCOS_Printf("%d %d %d\n", current_angle, accelBuffer[i], gyroBuffer[i]);
angleBuffer[i] = imu_current_angle;
i=(i+1)%(sizeof(angleBuffer)/sizeof(angleBuffer[0]));
} }
/** /**
* Initialize I2C and MPU * Initialize I2C and MPU
*/ */
int mpu9250_Imu_Init(void *pdata){ int mpu9250_Imu_Init(void *pdata){
//mpu9250_Write_Reg();
mpu9250_Iic_Init(); mpu9250_Iic_Init();
mpu9250_Write_Reg(29, 5); mpu9250_Write_Reg(29, 5); // accel dlpf 10Hz
mpu9250_Write_Reg(26, 5); // gyro dlpf 10Hz
//uint8_t rxbuf[20];
uint16_t i=0;
while(1){
mpu9250_Read_Data(0x3B, 6, (u8*)accelData);
mpu9250_Read_Data(0x43, 6, (u8*)gyroData);
accelBuffer[i]=(int16_t)((accelData[0]&0xFF)<<8 | (accelData[0]&0xFF00)>>8);
gyroBuffer[i]=(int16_t)((gyroData[1]&0xFF)<<8 | (gyroData[1]&0xFF00)>>8);
i=(i+1)%(sizeof(accelBuffer)/sizeof(accelBuffer[0]));
OSTimeDly(1);
}
} }

4
src/APP/Aufgabe7/ps7/core0/main.c
View File

@@ -26,6 +26,10 @@ void InitDoneCallback(void * p_arg) {
(void) p_arg; (void) p_arg;
UCOS_Print("OS started!\r\n"); UCOS_Print("OS started!\r\n");
GT_Init(); GT_Init();
uint8_t err;
OSTaskNameSet(31, "IMU Task", &err);
OSTaskNameSet(32, "Timer Task", &err);
OSTaskNameSet(33, "PID Task", &err);
} }
int main(void) { int main(void) {