#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "hi_types_base.h"
#include "hi_errno.h"
#include "hi_io.h"
#include "hi_gpio.h"
#include "hi_i2c.h"
#include "hi_time.h"
#include "hi_watchdog.h"
#include "engine.h"
#include "pca9685.h"
#include "display.h"
// 舵机数
#define SE_PN 2
// 舵机参数
struct SE_Par
{
hi_u16 an1; // 当前角度
hi_u16 an2; // 转向角度
hi_u16 lef; // 左边界
hi_u16 mid; // 中间
hi_u16 rig; // 右边界
hi_u8 pin; // 端口
hi_u8 run; // 运行 0 1
hi_u8 spa; // 跨度 0 1 10
hi_u8 del; // 延时 0 1
hi_u8 dir; // 方向 0 1
};
struct SE_Par se_ps[SE_PN] = {0}; // 舵机参数
hi_u8 se_pi = 0; // 舵机端口
// 舵机参数初始化
hi_void Engine_Init(hi_void)
{
for(hi_u8 i = 0; i < SE_PN; i++)
{
se_ps[i].pin = i;
se_ps[i].run = 0;
se_ps[i].spa = 0;
se_ps[i].dir = 0;
se_ps[i].del = 0;
se_ps[i].an1 = 135;
se_ps[i].an2 = 135;
se_ps[i].lef = 0;
se_ps[i].mid = 135;
se_ps[i].rig = 270;
PCA9685_Angle(se_ps[i].pin, se_ps[i].an1, 270);
}
Display_SE_Data();
}
// 显示舵机运行状态
hi_void Display_SE_Data(hi_void)
{
Display_Pin(SE_PN, se_pi);
Display_State(se_ps[se_pi].spa, se_ps[se_pi].del, se_ps[se_pi].dir, se_ps[se_pi].run);
Display_Angle(se_ps[se_pi].dir, se_ps[se_pi].an1, se_ps[se_pi].an2);
Display_Boundary(se_ps[se_pi].dir, se_ps[se_pi].lef, se_ps[se_pi].mid, se_ps[se_pi].rig);
}
// 左转
hi_u16 Turn_Left(struct SE_Par par)
{
hi_u16 ang = 0;
if(par.spa == 0)
{
ang = se_ps[se_pi].lef;
}else{
if(par.an1 > (par.spa + par.lef))
{
ang = par.an1 - par.spa;
}else{
ang = se_ps[se_pi].lef;
}
}
return ang;
}
// 右转
hi_u16 Turn_Right(struct SE_Par par)
{
hi_u16 ang = 0;
if(par.spa == 0)
{
ang = par.rig;
}else{
ang = par.an1 + par.spa;
if(ang > par.rig)
{
ang = par.rig;
}
}
return ang;
}
// 遥控 控制
hi_u8 RC_Control(hi_u8 con)
{
hi_u8 ret = 1;
// 设置跨度
// 0xa2, 1
if(con == 0xa2)
{
printf(" 1 : \n");
se_ps[se_pi].spa = 0;
}
// 0x62, 2
if(con == 0x62)
{
printf(" 2 : \n");
se_ps[se_pi].spa = 1;
}
// 0xe2, 3
if(con == 0xe2)
{
printf(" 3 : \n");
se_ps[se_pi].spa = 10;
}
// 设置边界
// 左
// 0x22, 4
if(con == 0x22)
{
printf(" 4 : \n");
if(se_ps[se_pi].dir == 0)
{
se_ps[se_pi].lef = se_ps[se_pi].an1;
}
if(se_ps[se_pi].dir == 1)
{
se_ps[se_pi].rig = se_ps[se_pi].an1;
}
}
// 中
// 0x02, 5
if(con == 0x02)
{
printf(" 5 : \n");
se_ps[se_pi].mid = se_ps[se_pi].an1;
}
// 右
// 0xc2, 6
if(con == 0xc2)
{
printf(" 6 : \n");
if(se_ps[se_pi].dir == 0)
{
se_ps[se_pi].rig = se_ps[se_pi].an1;
}
if(se_ps[se_pi].dir == 1)
{
se_ps[se_pi].lef = se_ps[se_pi].an1;
}
}
// 旋转方向设置
// 0xe0, 7
if(con == 0xe0)
{
printf(" 7 : \n");
if(se_ps[se_pi].dir == 0)
{
se_ps[se_pi].dir = 1;
}else{
se_ps[se_pi].dir = 0;
}
}
// 0xa8, 8
if(con == 0xa8)
{
printf(" 8 : \n");
}
// 运行延时开关
// 0x90, 9
if(con == 0x90)
{
printf(" 9 : \n");
if(se_ps[se_pi].del == 0)
{
se_ps[se_pi].del = 1;
}else{
se_ps[se_pi].del = 0;
}
}
// 运行到中间
// 0x98, 0
if(con == 0x98)
{
printf(" 0 : \n");
se_ps[se_pi].an2 = se_ps[se_pi].mid;
se_ps[se_pi].run = 1;
}
// 退出
// 0x68, *
if(con == 0x68)
{
printf(" * : \n");
ret = 0;
}
// 重置
// 0xb0, #
if(con == 0xb0)
{
printf(" # : \n");
se_ps[se_pi].an1 = 135;
se_ps[se_pi].an2 = 135;
se_ps[se_pi].lef = 0;
se_ps[se_pi].mid = 135;
se_ps[se_pi].rig = 270;
}
// 端口切换
// 0x18, up
if(con == 0x18)
{
printf(" up : \n");
if(se_pi < (SE_PN - 1))
{
se_pi++;
}
}
// 0x4a, down
if(con == 0x4a)
{
printf(" down : \n");
if(se_pi > 0)
{
se_pi--;
}
}
// 向左运行
// 0x10, left
if(con == 0x10)
{
printf(" left : \n");
if(se_ps[se_pi].dir == 0)
{
se_ps[se_pi].an2 = Turn_Left(se_ps[se_pi]);
}
if(se_ps[se_pi].dir == 1)
{
se_ps[se_pi].an2 = Turn_Right(se_ps[se_pi]);
}
se_ps[se_pi].run = 1;
}
// 向右运行
// 0x5a, right
if(con == 0x5a)
{
printf(" right : \n");
if(se_ps[se_pi].dir == 0)
{
se_ps[se_pi].an2 = Turn_Right(se_ps[se_pi]);
}
if(se_ps[se_pi].dir == 1)
{
se_ps[se_pi].an2 = Turn_Left(se_ps[se_pi]);
}
se_ps[se_pi].run = 1;
}
// 运行停止
// 0x38, OK
if(con == 0x38)
{
printf(" OK : \n");
if(se_ps[se_pi].run == 1)
{
se_ps[se_pi].run = 0;
se_ps[se_pi].an2 = se_ps[se_pi].an1;
}
}
// 显示信息
Display_SE_Data();
return ret;
}
// 舵机 运行
hi_void Engine_Run(hi_void)
{
if(se_ps[se_pi].an1 == se_ps[se_pi].an2)
{
se_ps[se_pi].run = 0;
Display_State(se_ps[se_pi].spa, se_ps[se_pi].del, se_ps[se_pi].dir, se_ps[se_pi].run);
return;
}
if(se_ps[se_pi].an1 > se_ps[se_pi].an2)
{
se_ps[se_pi].an1--;
}
if(se_ps[se_pi].an1 < se_ps[se_pi].an2)
{
se_ps[se_pi].an1++;
}
if(se_ps[se_pi].del == 0)
{
PCA9685_Angle_270(se_ps[se_pi].pin, se_ps[se_pi].an1, 270);
Display_Angle(se_ps[se_pi].dir, se_ps[se_pi].an1, se_ps[se_pi].an2);
}
if(se_ps[se_pi].del == 1)
{
PCA9685_Angle_270(se_ps[se_pi].pin, se_ps[se_pi].an2, 270);
se_ps[se_pi].an1 = se_ps[se_pi].an2;
se_ps[se_pi].run = 0;
Display_State(se_ps[se_pi].spa, se_ps[se_pi].del, se_ps[se_pi].dir, se_ps[se_pi].run);
Display_Angle(se_ps[se_pi].dir, se_ps[se_pi].an1, se_ps[se_pi].an2);
}
}
hi_u8 SE_Get_Pin(hi_void)
{
return se_ps[se_pi].pin;
}
hi_u8 SE_Get_Run(hi_void)
{
return se_ps[se_pi].run;
}
hi_u8 SE_Get_Spa(hi_void)
{
return se_ps[se_pi].spa;
}
hi_u8 SE_Get_Del(hi_void)
{
return se_ps[se_pi].del;
}
hi_u16 SE_Get_An1(hi_void)
{
return se_ps[se_pi].an1;
}
hi_u16 SE_Get_An2(hi_void)
{
return se_ps[se_pi].an2;
}
hi_void SE_Set_Run(hi_u8 r)
{
se_ps[se_pi].run = r;
}
hi_void SE_Set_An1(hi_u16 a)
{
se_ps[se_pi].an1 = a;
}
hi_void SE_Set_An2(hi_u16 a)
{
se_ps[se_pi].an2 = a;
}