void SysInit(void)
{
HalInit();
MProtoInit(sysEventHandle);
MotorInit(sysMotorEventHandle);
sysMotorInit();
}
/********************************************
飞控主函数入口
功能：
1.初始化各个硬件
2.初始化系统参数
3.开定时器4等待数据中断到来
4.开定时器3串口广播实时姿态以及相关信息
********************************************/
int main(void)
{
//int i;
SystemClock_HSE(9); //系统时钟初始化，时钟源外部晶振HSE
//SystemClock_HSI(4); //系统时钟初始化，时钟源内部HSI
UART1_init(SysClock,115200); //串口1初始化
NVIC_INIT(); //中断初始化
STMFLASH_Unlock(); //内部flash解锁
LedInit(); //IO初始化
delay_init(SysClock); //滴答延时初始化
BT_PowerInit(); //蓝牙电源初始化完成，默认关闭
MotorInit(); //马达初始化
BatteryCheckInit(); //电池电压监测初始化
IIC_Init(); //IIC初始化
MPU6050_DMP_Initialize(); //初始化DMP引擎
//HMC5883L_SetUp(); //初始化磁力计HMC5883L
PID_INIT(); //PID参数初始化
ParameterRead(); //Flash参数读取
NRF24L01_INIT(); //NRF24L01初始化
SetRX_Mode(); //设无线模块为接收模式
PowerOn(); //开机等待
BT_on(); //蓝牙开
TIM3_Init(SysClock,10); //定时器3初始化，调试串口输出
TIM4_Init(SysClock,50); //定时器4初始化，定时采样传感器数据，更新PID输出
while (1); //等待数据更新中断到来
}
//*****************************************************************************
// Start Main
//*****************************************************************************
int main(void) {
char data_in[128];
//***************************************************************************
// Setting the system Clock
//
// 400 MHz PLL driven by 16 MHz external crystal
// builtin /2 from PLL, then /4 --> 50Hz Clock speed
SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
//
//***************************************************************************
InitUART3();
MotorInit(); //Initialize Motors
LineInit(); //Initialize Line Sensors
ultrasonicInit(); //Initialize UltraSonic
IRdsitinit(); //Initialize IR Distance Sensor
colorinit(); //Initialize Color Sensor
FollowINIT(); //Initialize timer interrupts for following
ShaftEncoder_Init();
IntMasterEnable(); //Enable Interrupts
UARTprintf("An & Paolina's Robot says Hello\n\r");
while (1) {
UARTgets(data_in,128);
MB_Get((unsigned char *)data_in);
Interpret(data_in);
}
}
int main() {
// Optical__value_all
u8 Optical_value_all;
// Optical__value_signal
u8 Optical_value_signal;
int Speed, Dir;
float Ultrasonic_value_all[3];
int i;
init_platform();
OptInit();
MotorInit();
BluetoothInit();
while (1) {
Optical_value_all = OptGetAll();
printf("optical_all : %X \r\n", Optical_value_all);
OptGetSingle(Opt1, &Optical_value_signal);
printf("optical_CH1 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt2, &Optical_value_signal);
printf("optical_CH2 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt3, &Optical_value_signal);
printf("optical_CH3 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt4, &Optical_value_signal);
printf("optical_CH4 : %X \r\n", Optical_value_signal);
OptGetSingle(Opt5, &Optical_value_signal);
printf("optical_CH5 : %X \r\n\r\n", Optical_value_signal);
UltraGetAll(Ultrasonic_value_all);
for (i = 0; i < 3; i++) {
printf("u%d : %f mm\r\n", i, Ultrasonic_value_all[i]);
usleep(1000);
}
printf("\r\n");
SetMotorSpeed(MotorL, 30);
SetMotorSpeed(MotorR, -80);
Speed = GetMotorSpeed(MotorL);
printf("MotorL Speed : %d \r\n", Speed);
Speed = GetMotorSpeed(MotorR);
printf("MotorR Speed : %d \r\n", Speed);
Dir = GetMotorDir(MotorL);
printf("MotorL Dir : %d \r\n", Dir);
Dir = GetMotorDir(MotorR);
printf("MotorR Dir : %d \r\n\r\n", Dir);
usleep(50 * 1000);
BluetoothSend("Hello World\r\n",11);
}
cleanup_platform();
return 0;
}
int main(void)
{
delay_init();
GPIOCLKInit();
UserLEDInit();
LCD1602Init();
//IRCtrolInit();
TIM2_Init();
MotorInit();
ServoInit();
RedRayInit();
//USART3Conf(9600);
while(1)
{
if(tick_5ms >= 5)
{
tick_5ms = 0;
tick_200ms++;
if(tick_200ms >= 40)
{
tick_200ms = 0;
LEDToggle(LED_PIN);
}
// continue_time--;//200ms 无接收指令就停车
// if(continue_time == 0)
// {
// continue_time = 1;
// CarStop();
// }
//do something
SearchRun();
if(ctrl_comm_last != ctrl_comm)//指令发生变化
{
ctrl_comm_last = ctrl_comm;
switch(ctrl_comm)
{
case COMM_UP: CarGo();break;
case COMM_DOWN: CarBack();break;
case COMM_LEFT: CarLeft();break;
case COMM_RIGHT: CarRight();break;
case COMM_STOP: CarStop();break;
default : break;
}
Delayms(10);//防抖
LCD1602WriteCommand(ctrl_comm);
}
}
}
}
int main(void)
{
delay_init();
GPIOCLKInit();
UserLEDInit();
//LCD1602Init();
//IRCtrolInit();
TIM2_Init();
MotorInit();
UltraSoundInit();
//RedRayInit();
ServoInit();
while(1)
{
if(tick_5ms >= 5)
{
tick_5ms = 0;
tick_200ms++;
if(tick_200ms >= 40)
{
tick_200ms = 0;
LEDToggle(LED_PIN);
//LCD1602WriteDistance(distance_cm);//更新距离
}
// continue_time--;//200ms 无接收指令就停车
// if(continue_time == 0)
// {
// continue_time = 1;
// CarStop();
// }
//do something
Distance();//计算距离
BarrierProc();
// if(ctrl_comm_last != ctrl_comm)//接收到红外信号
// {
// ctrl_comm_last = ctrl_comm;
// switch(ctrl_comm)
// {
// case COMM_UP: CarGo();break;
// case COMM_DOWN: CarBack();break;
// case COMM_LEFT: CarLeft();break;
// case COMM_RIGHT: CarRight();break;
// case COMM_STOP: CarStop();break;
// default : break;
// }
// LCD1602WriteCommand(ctrl_comm);
// }
}
}
}
int main(void)
{
uint32_t current_time = 0;
SystemInit();
GpioInit();
//AdcInit();
TimerInit(0, 1ul * _millisecond);
CallbackRegister(MotorHandler, 10ul * _millisecond);
CallbackEnable(MotorHandler);
TimerEnable(0);
EncoderInit();
MotorInit();
MotorStart();
I2cInit(SENSOR_BUS);
current_time = now;
while(now < current_time + 1000);
// center the wheels if they aren't already
set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70);
set_motor_position(MOTOR_BACK_FRONT, BF_CENTER, 72);
while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING);
current_time = now;
while(now < current_time + 5000);
// offset one motor
set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70);
set_motor_position(MOTOR_BACK_FRONT, BF_CENTER + 50, 72);
while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING);
current_time = now;
while(now < current_time + 1000);
while (1)
{
if (get_ir_sen())
{
set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER + 50, -70);
set_motor_position(MOTOR_BACK_FRONT, BF_CENTER, -72);
while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING);
}
else
{
set_motor_position(MOTOR_FRONT_FRONT, FF_CENTER, 70);
set_motor_position(MOTOR_BACK_FRONT, BF_CENTER + 50, 72);
while (motor[MOTOR_FRONT_FRONT].state == MOTOR_MOVING && motor[MOTOR_BACK_FRONT].state == MOTOR_MOVING);
}
}
return 0;
}
void MotorPidInit()
{
MotorInit();
FreecaleConfig.Config.Motor.Pid.Pid.Target = 0;
FreecaleConfig.Config.Motor.Pid.Pid.g = 0;
FreecaleConfig.Config.Motor.Pid.Pid.Imax = 0;
FreecaleConfig.Config.Motor.Pid.Pid.Integral = 0;
FreecaleConfig.Config.Motor.Pid.Pid.last_error = 0;
FreecaleConfig.Config.Motor.Pid.Pid.Now = 0;
FreecaleConfig.Config.Motor.Pid.Pid.P = MotorPIDDefaultP;
FreecaleConfig.Config.Motor.Pid.Pid.I = MotorPIDDefaultI;
FreecaleConfig.Config.Motor.Pid.Pid.D = MotorPIDDefaultD;
}
int main(void)
{
delay_init();
GPIOCLKInit();
UserLEDInit();
LCD1602Init();
IRCtrolInit();
TIM2_Init();
MotorInit();
ServoInit();
while(1)
{
if(tick_5ms >= 5)
{
tick_5ms = 0;
tick_200ms++;
if(tick_200ms >= 40)
{
tick_200ms = 0;
LEDToggle(LED_PIN);
}
continue_time--;//200ms 无接收指令就停车
if(continue_time == 0)
{
continue_time = 1;
CarStop();
}
//do something
if(ir_rec_flag == 1)//接收到红外信号
{
ir_rec_flag = 0;
switch(ctrl_comm)
{
case COMM_UP: CarGo();break;
case COMM_DOWN: CarBack();break;
case COMM_LEFT: CarLeft();break;
case COMM_RIGHT: CarRight();break;
case COMM_STOP: CarStop();break;
default : break;
}
LCD1602WriteCommand(ctrl_comm);
}
}
}
}
void main(void) {
ADCInit();//Start Analog to digital converter
MotorInit();//Start the motor controller
TimerPWMInit();//Start the generation of PWM signal using timers
LCDInit(LS_NONE); //Initialize the LCD Module
parallelInit();//Initialize the reception from the Arduino over the parallel port
SetMotorLeft(0);//Start motor left at speed 0
SetMotorRight(0);//Start motor right at speed 0
/* Endless loop*/
while(1) {
ADCInitiateConversion();//One analog read.
Go();//Take decissions to avoid obstacles. And send orders to obstacles.
}
}
/********************************************
飞控主函数入口
功能：
1.初始化各个硬件
2.初始化系统参数
3.开定时器4等待数据中断到来
4.开定时器3串口广播实时姿态以及相关信息
********************************************/
int main(void)
{
SystemClock(9); //系统时钟初始化
UART1_init(SysClock,115200); //串口1初始化
NVIC_INIT(); //中断初始化
STMFLASH_Unlock(); //内部flash解锁
LedInit(); //IO初始化
delay_init(SysClock); //滴答延时初始化
BT_PowerInit(); //蓝牙电源初始化完成，默认关闭
MotorInit(); //马达初始化
BatteryCheckInit(); //电池电压监测初始化
IIC_Init(); //IIC初始化
MPU6050_DMP_Initialize(); //初始化DMP引擎
PID_INIT(); //PID参数初始化
ParameterRead(); //Flash参数读取
NRF24L01_INIT(); //NRF24L01初始化
SetRX_Mode(); //设无线模块为接收模式
PowerOn(); //开机等待
BT_on(); //蓝牙开
TIM3_Init(36,2000); //定时器3初始化，调试串口输出
TIM4_Init(36,1000); //定时器4初始化，定时采样传感器数据，更新PID输出
while (1) //等待数据更新中断到来
{
// switch(UART1_Get_Char())//检测串口发送数据，做相应处理
// {
// case 'w':
// LedA_on;LedB_on;LedC_on;LedD_on;
// //TxBuf[0]++;
// //UART1_Put_Package(TxBuf);
// LedA_on;LedB_on;LedC_on;LedD_on;
// MotoPWM+=10;
// UART1_Put_Char(0x5a);
// break;
// case 's':
// LedA_off;LedB_off;LedC_off;LedD_off;
// UART1_Put_Char(0xaa);
// MotoPWM-=10;
// break;
// default :break;
// }
// MotorPwmFlash(MotoPWM,MotoPWM,MotoPWM,MotoPWM);
//
}
}
/********************************************************************************************************
Function Name: main
Description :
Inputs : None
Outputs : None
Notes :
Revision :
********************************************************************************************************/
int main(void)
{
SystemInit(); //ϵͳʱÖÓ³õʼ»¯
InputIOInit(); //Êý×ÖÊäÈë³õʼ»¯
OutputIOInit();//Êý×ÖÊä³ö³õʼ»¯
SensorInit(); //Ä£ÄâÊäÈë³õʼ»¯
ZLG7290Init(); //¼üÅ̳õʼ»¯
MotorInit(); //µç»ú³õʼ»¯
ServoInit(); //¶æ»ú³õʼ»¯
EncoderInit(); //±àÂëÆ÷³õʼ»¯
/*
V_PIDInit(); //µç»ú±Õ»·Êä³ö
*/
SystemStart();
//UsartInit(); //´®¿Ú³õʼ»¯
}
int main(void){
int log = 0;
//Start Switch
// DDRA = 0x00;
// PORTA = 0x12;
//Start PORT A for switch and IR sensors
DDRA = 0xFC;
PORTA = 0xFE;
//LED Initial
DDRC = 0x7F;
PORTC = 0x7E;
DDRD = 0x70;
PORTD = 0x11;
MotorInit();
initSerial();
char * readData = NULL;
int isFinish = 0;
sensorInit();
if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
while(1){
sensorTest(0);
sensorTest(1);
sensorTest(2);
setMode();
if( checkSerialRead() > 0 ){
readData = getReadBuffer();
if( readData != NULL ){
// printf( "readData=%s\n", &readData[0] );
split( &readData[0] );
switch( serCmd[0] ){
case EVT_ACTION:
ServoControl( serCmd[1] );
// setSpeedTest( serCmd[1] );
sendAck(1);
break;
case EVT_START_MOTION:
startMotion( serCmd[1], serCmd[2] );
PORTC = ~(1 << (LED_MAX - 2));
sendAck(1);
break;
case EVT_STOP_MOTION:
stopMotion();
sendAck(1);
break;
case EVT_FORCE_MOTION:
forceMotion( serCmd[1], serCmd[2] );
break;
case EVT_GET_NOW_ANGLE:
getAngle();
break;
case EVT_SET_ANGLE:
setAngle();
case EVT_GET_ACT_ANGLE:
if( serCmd[1] >= ACT_MAX ){
sendAck(0);
}else{
sendActAngle(serCmd[1]);
}
break;
case EVT_GET_LOAD:
getLoad();
// printf( "%d\n", movingTime );
break;
case EVT_GET_VOLTAGE:
getVoltage();
break;
case EVT_TORQUE_DISABLE:
dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
break;
case EVT_WATCH_DOG:
watchDogCnt = 0;
break;
case EVT_MOTION_EDIT:
break;
case 999:
// printf( "finish\n");
sendAck(999);
isFinish = 1;
break;
default:
sendAck(0);
}
if( isFinish > 0 ){
MotorControl( 0, 0 );
break;
}
memset( readData, 0x00, SERIAL_BUFFER_SIZE );
}
}
memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE );
if (~PINA & SW_START ) {
if (log == 1) printf( "main() 0\n");
if( iStart > 0 ){
iStart = 0;
PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY;
if (isCaptureMode != 1) ServoControl( 0 );
}
}else{
if( iStart == 0 ){
PORTC &= ~LED_PLAY;
iStart = 1;
}
if( modeWait <= 0 ){
if (log == 1) printf( "main() 1\n");
setModeAction();
if (isMovetest == 1) {
moveTest();
} else {
move();
}
}else{
if (log == 1) printf( "main() 2\n");
modeWait -= MAIN_DELAY;
}
}
if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) {
if (log == 1) printf( "### main() sensorValue[0] == 0\n");
PORTC |= LED_PROGRAM; //edit
}else if (sensorValueOld[0] != sensorValue[0]){
if (log == 1) printf( "### main() sensorValue[0] == 1\n");
PORTC &= ~LED_PROGRAM; //edit
}
if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) {
if (log == 1) printf( "### main() sensorValue[1] == 0\n");
PORTC |= LED_MANAGE; //mon
}else if (sensorValueOld[1] != sensorValue[1]){
if (log == 1) printf( "### main() sensorValue[1] == 1\n");
PORTC &= ~LED_MANAGE; //mon
}
if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) {
if (log == 1) printf( "### main() sensorValue[2] == 0\n");
PORTC |= LED_AUX; //AUX
}else if (sensorValueOld[2] != sensorValue[2]){
if (log == 1) printf( "### main() sensorValue[2] == 1\n");
PORTC &= ~LED_AUX; //AUX
}
sensorValueOld[0] = sensorValue[0];
sensorValueOld[1] = sensorValue[1];
sensorValueOld[2] = sensorValue[2];
// walk pattern LED
// brinkLED();
_delay_ms(MAIN_DELAY);
watchDogCnt++;
caputureCount1++;
if (caputureCount1 == 25){
if (isCaptureMode == 1) getAngle();
caputureCount1 = 0;
}
}
}
void dev_init(void)
{
Key_value_reset();
Protocol_data_init();
#if DEBUG_UART_EN
DbgPrintf("\r\n===dev_init===\r\n");
#endif
#if ENABLE_WDT
et_WDT_init();
#endif
#if DEBUG_UART_EN
//DbgPrintf("et_Z8_powerCtr OK\r\n");
#endif
#if ENABLE_ALARM
MotorInit();
#endif
#if DEBUG_UART_EN
//DbgPrintf("MotorInit--->ok\r\n");
#endif
battery_adc_dev_init();
#if DEBUG_UART_EN
//DbgPrintf("adc init OK\r\n");
#endif
#if ENABLE_ALARM
alarm_init();
#endif
#if DEBUG_UART_EN
//DbgPrintf("adc init OK\r\n");
#endif
system_time_init();
#if DEBUG_UART_EN
// DbgPrintf("rtc init OK\r\n");
#endif
ETTimer2Init();
#if DEBUG_UART_EN
// DbgPrintf("ETTimer2Init OK\r\n");
#endif
USB_detect_init();
#if DEBUG_UART_EN
//DbgPrintf("USB_detect_init OK\r\n");
#endif
#if ENABLE_PEDOMETER
#ifdef ENABLE_NEW_BOARD
sensor_GPIO_i2c_init();
#endif
gSensor_init();
#endif
#if DEBUG_UART_EN
//DbgPrintf("gSensor_init OK\r\n");
#endif
ETTimer1Init();
#if ENABLE_DISPLAY
et_i2c_init();
LCD_init();
#if DEBUG_UART_EN
DbgPrintf("lcd init OK\r\n");
#endif
#endif
//app_arthmetic_set_total_steps(999990);
}
