void motor_Init(void)
{
vsm_Init();
dbg_Init();
motor = sys_malloc(sizeof(motor_t));
/*default smo_motor para, change me!!!*/
motor->rs = (short) NOR_RES(65.0f);
motor->ld = (short) NOR_IND(49.0f);
motor->lq = (short) NOR_IND(49.0f);
motor->pn = 8;
motor->start_time = CONFIG_MOTOR_START_TIME;
motor->start_speed = NOR_SPEED(RPM_TO_SPEED(CONFIG_MOTOR_START_RPM));
motor->start_torque = NOR_AMP(CONFIG_MOTOR_START_CURRENT);
stm = MOTOR_IDLE;
led_off(LED_RED);
led_on(LED_GREEN);
pid_speed = pid_Init();
pid_torque = pid_Init();
pid_flux = pid_Init();
pid_SetRef(pid_speed, 0);
pid_SetRef(pid_torque, 0);
pid_SetRef(pid_flux, 0);
smo_Init();
}
void pid_strategy_init()
{
//File Output
fp = fopen("flightLog.csv", "wb");
if(fp==NULL)
{
printf("File open failed\n");
return;
}
fprintf(fp,"Log Count,Time Stamp,Acceleration x,Acceleration y,"
"Acceleration z,Gyro x,Gyro y,Gyro z,Vertical Speed,"
"Desired Height,Actual Height,Throttle,Desired Pitch,"
"Actual Pitch,Pitch Adjustment,Desired Roll,Actual Roll,"
"Roll Adjustment,Desired Yaw,Actual Yaw,Yaw Adjustment,"
"Magnet X,Magnet Y,Magnet Z,Bearing,Horizontal xv,Horizontal yv,"
/*"Blob x-coord,Blob y-coord*/"\n");
//init pid pitch/roll
pid_Init(&pid_roll, 0.4, 0.000, -0.05, 0);
pid_Init(&pid_pitch, 0.4, 0.000, -0.05, 0);
pid_Init(&pid_yaw, 0.8, 0.008, -0.065, 0);
/** @todo these need an i-part */
//pid_Init(&pid_hor_vel_x, 0.01, 0.0, 0, 0.1);
//pid_Init(&pid_hor_vel_y, 0.01, 0.0, 0, 0.1);
pid_Init(&pid_h, 0.26, 0.023, -0.014, 1);
throttle = 0.00;
}
int ctl_Init(char *client_addr)
{
int rc;
//defaults from AR.Drone app: pitch,roll max=12deg; yawspeed max=100deg/sec; height limit=on; vertical speed max=700mm/sec;
setpoint.pitch_roll_max=DEG2RAD(12); //degrees
//setpoint.yawsp_max=DEG2RAD(100); //degrees/sec
setpoint.h_max=600; //cm
setpoint.h_min=40; //cm
setpoint.throttle_hover=0.46;
setpoint.throttle_min=0.30;
setpoint.throttle_max=0.85;
//init pid pitch/roll
pid_Init(&pid_roll, 0.4,0,-0.05,0);//0.5
pid_Init(&pid_pitch, 0.4,0,-0.05,0);//0.5
pid_Init(&pid_yaw, 0.8,0,-0.06,0);//1.00
//pid_Init(&pid_yaw, 0,0,0,0);//1.00
pid_Init(&pid_h, 0.003,0,-0.001,1);//0.0005
// pid_Init(&pid_x, 0.01,0,0,0);//0.0005
// pid_Init(&pid_y, 0.01,0,0,0);//0.0005
throttle=0.00;
//Attitude Estimate
rc = att_Init(&att);
if(rc) return rc;
//udp logger
udpClient_Init(&udpNavLog, client_addr, 7778);
//navLog_Send();
printf("udpClient_Init\n", rc);
//start motor thread
rc = mot_Init();
if(rc) return rc;
vid.device = (char*)"/dev/video1";
vid.w=176;
vid.h=144;
vid.n_buffers = 4;
video_Init(&vid);
img_old = video_CreateImage(&vid);
img_new = video_CreateImage(&vid);
video_GrabImage(&vid, img_old);
//start ctl thread
rc = pthread_create(&ctl_thread, NULL, ctl_thread_main, NULL);
if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
return 202;
}
}
int ctl_Init(char *client_addr)
{
int rc;
//----- Initialize PID controller -----//
//some saturation for controller
setpoint.pitch_roll_max=DEG2RAD(10); //degrees
setpoint.throttle_min=0.05;//0.5
setpoint.throttle_max=0.95; //0.85
//init pid pitch/roll //Kp, Ki, Kd, imax
pid_Init(&pid_roll, 0.33,0.0,0.25,1); //[0.35 0.25] with additional part [0.33 0.25] without ...
pid_Init(&pid_pitch, 0.33,0.0,0.25,1);
throttle=0.000;
//----- Initialize AHRS system -----//
printf("Initialize Attitude.. \n");
//rc = att_Init(&att);
//if(rc) return rc;
if(navdata_init()) printf("navdata initialized\n");
navdata_flattrim(&ahrsdata);
//printf("bias_after:%f\t%f\t%f\n",gyros_offset[0],gyros_offset[1],gyros_offset[2]);
sleep(1);
//----- Initialize UDP -----//
// Udp logger
printf("client\n");
udpClient_Init(&udpNavLog, "192.168.43.176", 9930); // Update the IP of ground station to send data
//udpClient_Init(&udpNavLog, "192.168.1.4", 9930);
//navLog_Send();
printf("udpClient_Init\n", rc);
//----- Start motor thread -----//
printf("Initialize motor\n");
rc = mot_Init();
if(rc) return rc;
//----- Start ctl thread -----//
printf("Initialize ctl_thread_main\n");
rc = pthread_create(&ctl_thread, NULL, ctl_thread_main, NULL);
if(rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
return 202;
}
else printf("rc = %d\n", rc);
}
int main(void)
{
Stm32_Clock_Init(9);
// u16 temp;
NVIC_Configuration();
UART1_init(72,115200);
delay_init();
//初始化串口1；
pid_Init();
LED_Init();
EXTIX_Init();
Adc_Init();
Motor_Init(999,0);
HC05_Init();
while(mpu_dmp_init());
// TIM5_Init(999,72);
TIM2_Int_Init(999,72);//定时器3配置，1ms中断一次
while(1)
{
// CtrlAttiAng();
// PWM_test();
// COMMPC_TEST();
// Get_angle();
// u2_printf("hell");
test_SendPC();//测试向PC发送协议数据，用于调控
}
}
void boom_init() {
TCCR1A = 0;
TCCR1B = 0;
// waveform generation = 0100 = CTC
TCCR1B |= (1<<WGM12);
TCCR1B |= 1<<CS10; // No prescler
TIMSK1 = _BV(OCIE1A); // enable oveflow interrupt
OCR1A = CYCLES_PER_TICK; // Set maximum value
TCNT1 = 0;
// Initialize the pid controllers
pid_Init(0.1, 0.0, 0.0, &pid[0]);
pid_Init(0.1, 0.0, 0.0, &pid[1]);
pid_Init(0.1, 0.0, 0.0, &pid[2]);
}
/*! \brief Init of PID controller demo
*/
void Init(void)
{
pid_Init(K_P * SCALING_FACTOR, K_I * SCALING_FACTOR , K_D * SCALING_FACTOR , &pidData);
// Set up timer, enable timer/counte 0 overflow interrupt
TCCR0A = (1<<CS00);
TIMSK0 = (1<<TOIE0);
TCNT0 = 0;
}
void pid_manager_init(void)
{
//uart_log(__func__);
uint8_t index;
for(index = 0; index < PID_COUNT; index++)
{
pid_array[index].pid_created = 0;
pid_array[index].p_addr = 0;
pid_array[index].i_addr = 0;
pid_array[index].d_addr = 0;
pid_Init(0,0,0,&(pid_array[index].pid));
}
}
int main() {
//task 6.1
pButtonCallback joystickButton = wrapper_pwm;
button_init(true); //debouncing, timer1 is initialized in this function
button_setJoystickButtonCallback(joystickButton);
pwm_init();
pwm_setDutyCycle(0); //initially motor idle
//task 6.2,6.3
lcd_init();
led_yellowInit();
led_greenInit();
scheduler_init(); // timer2_start() is called there.
motorFrequency_init(); //timer 5 and external interrupt settings
taskDescriptor td1; //= (taskDescriptor*) malloc(sizeof(taskDescriptor));
td1.task = &display;
//td.param = &; //void pointer point to void, param is of type void
td1.expire = 0;
td1.period = 500; //timer2 INT every 1ms
td1.execute = 0;
td1.next = NULL;
scheduler_add(&td1);
//task6.4
uart_init(57600); //debugging
pidData_t pidData;
pid_Init(P * SCALING_FACTOR, I* SCALING_FACTOR, D *SCALING_FACTOR, &pidData); //set p=0; i=1.5, d=0;
taskDescriptor td2; //= (taskDescriptor*) malloc(sizeof(taskDescriptor));
td2.task = &wrapper_pid;
td2.param = &pidData; //void pointer point to void, param is of type void
td2.expire = 0;
td2.period = 200; //timer2 INT every 1ms
td2.execute = 0;
td2.next = NULL;
scheduler_add(&td2);
sei();
scheduler_run();
return 0;
}
void setPidHight(float Kp, float Ki, float Kd){
pid_Init(&pid_h, Kp, Ki, Kd, 10);
}
void setPidYaw(float Kp, float Ki, float Kd){
pid_Init(&pid_yaw, Kp,Ki,Kd,0);
}
void setPidPitchRoll(float Kp, float Ki, float Kd){
//init pid pitch/roll
pid_Init(&pid_roll, Kp,Ki,Kd,10);//0.5
pid_Init(&pid_pitch, Kp,Ki,Kd,10);//0.5
}
//-----------------------------------------------------------------------------------------------------------------------------
//! @brief this task tests the functions
//! @return none
//! @author cho
//! @note
//! @todo
//-----------------------------------------------------------------------------------------------------------------------------
task main()
{
pid_Init();
}
static void avr_init(void)
// Initializes AVR controller
{
MAX6675S_DDR |= BV(MAX6675_CSPIN); // setup direction for CS pin
DISABLE_6675; // Set CS = high for MAX6675
initZerocross(); // init ext Int0
initTriac(); // init Triac pin & direction
#ifdef REG_PID
// initPID(&PID_data, 1, 0, 10, 10, -10);
//void pid_Init(int16_t p_factor, int16_t i_factor, int16_t d_factor, struct PID_DATA *pid);
pid_Init(P_FACTOR, I_FACTOR, D_FACTOR, &PID_data);
#endif
#ifdef REG_PD
initPD(&PD_data, 800, 1000);
#endif
//attach functions to timer interrupts
timerAttach(TIMER0OVERFLOW_INT, sample);
timerAttach(TIMER2OUTCOMPAREA_INT, stopTriac);
timerAttach(TIMER1OUTCOMPAREA_INT, fireTriac);
//enable output compare for timers
sbi(TIMSK0, TOIE0);
sbi(TIMSK1, OCIE1A);
sbi(TIMSK2, OCIE2A);
//set OCR values.
// OCR0A = PHASE_ANGLE_LIMIT_HIGH; //firing angle of triac
OCR2A = 1; //length of firing pulse
OCR1A = PHASE_ANGLE_LIMIT_HIGH; //temperature sample frequency
timer0SetPrescaler(TIMER_CLK_DIV1024); //start temp. sampling
/***************************************
* Timer0 runs at 24Mhz/1024 =
*
*
*
***************************************/
#ifndef DEBUG_SIM
#ifdef DEBUG_SER
// initialize the UART (serial port)
uartInit();
// set the baud rate of the UART for our debug/reporting output
uartSetBaudRate(57600);
// make all rprintf statements use uart for output
rprintfInit(uartSendByte);
// print a little intro message so we know things are working
rprintf("Reflow!\r\n");
// vt100SetCursorMode(1);
// vt100SetAttr(VT100_BLINK_OFF);
// vt100SetAttr(VT100_USCORE_OFF);
// vt100SetAttr(VT100_REVERSE);
//vt100ClearScreen();
// start command line
// goCmdline();
#endif
// initialize SPI interface
spiInit();
#endif
// SET_HALF_PHASE;
// SET_SKIP_PHASE;
// skips = 15;
//enable interrupts
sei();
return;
}
