/********************************************************************************************
**函数名称: light_control()
**函数功能: 追光函数
**输入参数: none
**输出参数: none
**返 回 值: none
**创 建 人: whble
**创建日期: 2013-7-25
*********************************************************************************************
**修 改 人:
**修改说明:
********************************************************************************************/
void light_control()
{
while(1)
{
DisplayListChar(0, 0, " light control ");
DisplayListChar(0, 1, " ");
if(flag!=4)break; //判断是否跳出追光程序
if(pwm==1)hight_votage=22;
if(pwm==2)hight_votage=8;
if(pwm==0)hight_votage=15;
if(light_left==0&&light_right==0) //追光前进
{
Motor_GoHead();
DisplayListChar(0, 1, " light gohead ");
}
if(light_left==1&&light_right==0) //追光右转
{
Motor_GoRightFast();
DisplayListChar(0, 1, " light goright ");
}
if(light_left==0&&light_right==1) //追光左转
{
Motor_GoLeftFast();
DisplayListChar(0, 1, " light goleft ");
}
if(light_left==1&&light_right==1) //追光停止
{
Motor_Stop();
DisplayListChar(0, 1, " light stop ");
}
}
}//end light_control
void Motor_Start(void)
{
uint8_t ii;
Motor_Stop();
Hall_Temp=HALL_DATA();
Hall_TempOld=Hall_Temp;
SVM_Angle=Hall_Angle[Hall_TempOld]+ANGLE_60D/2;
THallAngle=SVM_Angle;
PWM_Cnt=1000;
TRun_Flag=0;
TCnt_Num=0;
for(ii=0;ii<6;ii++)
{
TPWM_Cnt[ii+1]=0xFFFF;
TPWM_Num[ii+1]=0xFFFF;
}
Open_PWM();
Motor_State=RUN;
ShortI_Flag=0;
FOC_Flag=1;
SpeedCnt=5000;
SpeedData=0;
ID_Reference=0;
VQ_Reference=1000;
IQ_Reference=1000;
ID_PID_t.PI_Sum=0;
IQ_PID_t.PI_Sum=0;
SPEED_PID_t.PI_Sum=32768*2000;
}
void FailSafeLostRC(void)
{
if(getRC_Status() == STATUS_LOST)
{
Motor_Stop();
//command == MODEL_NORMAL;
}
}
void Main_Loop(void)
{
static uint8_t SpeedLoopcnt;
static uint32_t LsData1,LsData2;
if(T2ms_Flag)
{
T2ms_Flag=0;
ReadSignedADC0( &ReadADCParm );
SB_Temp=SB_FilterHe; //12bit
if(SB_Temp>200) //
{
if(Motor_State!=RUN)
{
Motor_Start();
}
}
else if(SB_Temp>190) ;
else
{
if(Motor_State!=WAIT) Motor_Stop();
}
if(++SpeedLoopcnt>1)
{
SetSpeed=SB_Temp+200; //100;//
if(SetSpeed>NOMINALSPEEDINRPM) SetSpeed=NOMINALSPEEDINRPM;
SpeedLoopcnt=0;
LsData1=60*PWMFREQUENCY;
LsData2=SpeedCnt*POLEPAIRS;
SpeedData=LsData1/LsData2;
#ifdef OPEN_I
LsData1=(Curr_q_d.qI_Component1*VQ_Reference);
LsData1/=32768;
if(LsData1>POWER_MAX) VQ_Reference-=100;
else
{
VQ_Reference=PID_Regulator(SetSpeed,SpeedData,&SPEED_PID_t);
}
#else
IQ_Reference=PID_Regulator(SetSpeed,SpeedData,&SPEED_PID_t);
#endif
}
}
if(T100ms_Flag)
{
//LED2Toggle();
T100ms_Flag=0;
}
}
void CommandProcess(void)
{
int command;
char mode;
// Read incoming control messages
//if (Serial_available() >= 2)
if(getUartData == TRUE)
{
char start=g_u8RecData[0];//UART_READ(UART);//Serial_read();
if (start == '@')
{// Start of new control message
printf("@HOOK \n");
//int command = UART_READ(UART);//Serial_read(); // Commands
command = g_u8RecData[1];
mode = g_u8RecData[2];
if (command == 's')
{
printf("@s ok \n");
// 's'tream output control
if (mode == 's')
{// 's'tart stream
//stream_mode = STREAM_START;
Motor_Start();
}
else if (mode == 'p')
{// 'p'ause stream
//stream_mode = STREAM_PAUSE;
Motor_Stop();
}
}
// else
// {
// if (report_format == REPORT_FORMAT_TEXT) {
// printf("Unknown command.\n");
// }
} // Skip character
getUartData = FALSE; //标记接收完成
}
}
void *Thread_Motor(void *_thread_arg) {
Thread_Arg *thread_arg = (Thread_Arg*)_thread_arg;
Command_Info command;
int ret = 0;
printf("--- thread motor ---\n");
if(wiringPiSetupGpio() == -1){
printf("---------wiringPiSetupGpio missing----------\r\n");
/* return; */
}
ret = Motor_main(&command, thread_arg);
printf("--- thread motor loop end ---\n\n");
pthread_mutex_lock(&thread_arg->mutex);
{
thread_arg->end_flag = 1;
}
pthread_mutex_unlock(&thread_arg->mutex);
Motor_Stop();
}
static int st_motor_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
{
switch (cmd) {
case MOTOR_Forward : {
OSTIMER_OFF;
MOTOR_Init();
Motor_Direction( Forward );
MOTOR_ReSetting(motor_speed); // 100us (default clk) = 10Hz
break;
}
case MOTOR_Reverse : {
OSTIMER_OFF;
MOTOR_Init();
Motor_Direction( Reverse );
MOTOR_ReSetting(motor_speed); // 100us (default clk) = 10Hz
break;
}
case MOTOR_SpeedUp : {
if(motor_speed > 200){
motor_speed -= 50; // Speed Up
printk("Speed up %d\n", motor_speed);
}
break;
}
case MOTOR_SpeedDown : {
motor_speed += 50; // Speed Down
break;
}
case MOTOR_STOP : {
Motor_Stop();
break;
}
default:
return 0;
}
return 0;
}
void main()
{
Sound_Init(&PORTC, 0); // Init Sound
while(PORTA.F4==1);
Sound_Play(2000,50); // 2 kHz sound ON RC0
Forward(255); // Call Forward
Delay_ms(2000);
Sound_Play(2000,50); // 2 kHz sound ON RC0
S_Left(255); // Call Spin Left
Delay_ms(800);
Sound_Play(2000,50); // 2 kHz sound ON RC0
Forward(255); // Call Forward
Delay_ms(2000);
Sound_Play(2000,50); // 2 kHz sound ON RC0
S_Right(255); // Call Spin Right
Delay_ms(800);
Sound_Play(2000,50); // 2 kHz sound ON RC0
Forward(255); // Call Forward
Delay_ms(2000);
Sound_Play(2000,50); // 2 kHz sound ON RC0
S_Left(255); // Call Spin Left
Delay_ms(800);
Sound_Play(2000,50); // 2 kHz sound ON RC0
Backward(255); // Call Backward
Delay_ms(1000);
Sound_Play(2000,50); // 2 kHz sound ON RC0
Motor_Stop(); // Stop all
}
static void _motorCtr(void)
{
Motor_Stop();
}
void loop()
{
static uint32_t nextTick = 0;
while(millis()<nextTick){}
nextTick = millis()+TICK_FRAME_PERIOD; //循环间隔FRAME
//处理蓝牙命令
CommandProcess();
//读取遥控命令
Comm_Process();
if(GetFrameCount()%10 == 0)
{
//读取姿态传感器数据
//读取欧拉角
#ifdef IMU_SW //软件姿态解算
//IMUSO3Thread();
#else
DMP_Routing(); //DMP 线程 所有的数据都在这里更新
#endif
//imu校准
// if(imuCaliFlag)
// {
// if(IMU_Calibrate())
// {
// imuCaliFlag=0;
// gParamsSaveEEPROMRequset=1; //请求记录到EEPROM
// imu.caliPass=1;
// }
// }
//PID二环角速度
//CtrlAttiRate();
//控制电机
//CtrlMotor();
}
if(GetFrameCount()%20 == 0)
{
//处理遥控数据
//PID一环角度控制
//CtrlAttiAng();
}
//10HZ,每100ms一次
//if(getTickCount()%100 == 0)
if(GetFrameCount()%1000 == 0)
{
//检测电池电量
BatteryCheck();
//printf("Convert result is %d\n", GetBatteryAD());
//遥控通信丢失处理
//更新LED灯状态
UpdateLED();
}
if(GetFrameCount()%100 == 0)
{
//printf("yaw=%d, roll=%d, pitch=%d \n",(int)imu.yaw, (int)imu.roll, (int)imu.pitch);
//printf("\n");
}
//故障保护
if(GetFrameCount() > 6000 && GetFrameCount() < 8000)
{
Motor_Start();
MotorPwmOutput(0,0,85,0);
}
// else if(GetFrameCount() >= 8000 && GetFrameCount() < 10000)
// {
// MotorPwmOutput(40,40,40,40);
// }
// else if(GetFrameCount() >= 10000 && GetFrameCount() < 12000)
// {
// MotorPwmOutput(60,60,60,60);
// }
// else if(GetFrameCount() >= 12000 && GetFrameCount() < 14000)
// {
// MotorPwmOutput(80,80,80,80);
// }
// else if(GetFrameCount() >= 14000 && GetFrameCount() < 16000)
// {
// MotorPwmOutput(100,100,100,100);
// }
else if(GetFrameCount() >= 40000 && !falg)
{
falg = true;
//MotorPwmOutput(0,0,0,0);
Motor_Stop();
}
IncFrameCount(1);
}
