void Left_Out_Side()
{
//if(LDC_SPI0_val < ldc0_side && LDC_SPI1_val < adjustment[8]+adjustment[0]+150 && LDC_SPI1_pre > LDC_SPI1_val) //左边 小于1748是说明 左边在铝膜上 不打死 右边小于1906 且 值在变小 说明 丢线了
if(LDC_SPI0_val < (adjustment[1]+20) && LDC_SPI1_val > adjustment[2]-60) //90 //70 //左边 小于1748是说明 左边在铝膜上 不打死 右边小于1906 且 值在变小 说明 丢线了
{
//LCD_P6x8Str(86,6,"leftOut");
FTM_PWM_Duty(FTM1,FTM_CH0,angle_to_period(STEERING_ANGLE_MIN));
while(1)
{
LDC_get();
if(flag) LDC_SPI1_val += adjustment[0];
else LDC_SPI0_val += adjustment[0];
//TEST_display();
Out_side_Motor_ctl();
//if(LDC_SPI0_val > adjustment[7] | LDC_SPI1_val > adjustment[8]-50)
if(LDC_SPI0_val > adjustment[1]+30) //50
{
// LCD_Fill(0x00);
break;
}
}
}
}
void Right_Out_Side()
{
//if(LDC_SPI1_val < 1756 && LDC_SPI0_val < 1907 && LDC_SPI0_pre > LDC_SPI0_val)
if(LDC_SPI0_val > adjustment[4]-60 && LDC_SPI1_val < (adjustment[3]+20)) //90 70 //左边 小于1748是说明 左边在铝膜上 不打死 右边小于1906 且 值在变小 说明 丢线了
{
FTM_PWM_Duty(FTM1,FTM_CH0,angle_to_period(STEERING_ANGLE_MAX));
//LCD_P6x8Str(90,6,"ROut");
while(1)
{
LDC_get();
if(flag) LDC_SPI1_val += adjustment[0];
else LDC_SPI0_val += adjustment[0];
//TEST_display();
Out_side_Motor_ctl();
//if(LDC_SPI0_val > 2240 | LDC_SPI1_val > 1740)
if( LDC_SPI1_val > adjustment[3]+30) //70
{
// LCD_Fill(0x00);
break;
}
}
}
}
void PIT1_IRQHandler(void)
{
PIT_Flag_Clear(PIT1); //清中断标志位
FTM_PWM_Duty(FTM0, CH2 ,10);
if(sin_count==16)
{
sin_count=0;
}
sin_count++;
}
/*************************************************************************
* 野火嵌入式开发工作室
* PWM实验LED测试
*
* 实验说明：野火PWM实验，用LED来测试占空比的变化。
*
* 实验操作：这里用 FTM1_CH0 产生 PWM 脉冲波
* 在 FTM.h 里，可以查到 FTM1_CH0 对应管脚为 PTA8
* 把 PTA8 接入 LED0，即 PTA8 和 PTD15 短接
*
* 实验效果：可以看到 LED0 由暗变亮，再突然暗，如此下去……
*
* 实验目的：测试 PWM 频率是否正确
*
* 修改时间：2012-2-29 已测试
*
* 备 注：野火Kinetis开发板的 LED0~3 ，分别接PTD15~PTD12 ，低电平点亮
* FTM.h 里有各个FTM通道所对应管脚的表格，方便查看
* 占空比传递进入的参数，要根据 FTM_PRECISON 的定义来选择
*************************************************************************/
void main()
{
u32 i;
FTM_PWM_init(FTM1, CH0, 35000, 100); //FTM1_CH0初始化PWM ：PA8
while(1)
{
for(i = 10; i > 1; i--)
{
FTM_PWM_Duty(FTM1, CH0, i * 10); //改变占空比，逐渐变小 ,LED 逐渐变亮 （低电平点亮）
time_delay_ms(100); //延时100ms
}
}
}
void Motor_PID()
{
//if(iErr - LastErr <= -1 || iErr - PreErr >= 1) return next;
thisErr = now - AimSpeed;
pErr = LastErr - thisErr;
iErr = thisErr;
dErr = thisErr - 2 * LastErr + PreErr;
out += p * pErr - i * iErr + d * dErr;
if(out >= 75000) out = 75000;
PreErr = LastErr;
LastErr = thisErr;
//printf("encode=%d\r\n",(int)guad_val);
//FTM_PWM_Duty(FTM0,FTM_CH2,tiffer*120);
//FTM_PWM_Duty(FTM0,FTM_CH2,60000);
//systick_delay_ms(5000);
FTM_PWM_Duty(FTM0,FTM_CH2,out);
//systick_delay_ms(1000);
// FTM_PWM_Duty(FTM0,FTM_CH2,80000);
// systick_delay_ms(1000);
//printf("tiffer: %d\n\r",(int)tiffer);
}
void pit3_system_loop(void)
{
switch (system_mode)
{ case 0:
if(gpio_get(PORTE, 6) == 0)
{
system_mode=99;
}
if(gpio_get(PORTE, 8) == 0)
{system_mode=98;}
if(gpio_get(PORTE, 9) == 0)
{system_mode=97;}
if(e<5000){e++;system_mode=0; //wait 5 sec
}
break;
case 99:
ideal_speed=300;
trankp=14;
leftkp=12;
rightkp=13;
leftkd=8;
rightkd=6;
system_mode=100;
break;
case 98:
ideal_speed=300;
trankp=13;
leftkp=13;
rightkp=11;
leftkd=9;
rightkd=7;
system_mode=100;
break;
case 97:
ideal_speed=180;
trankp=18;
leftkp=18;
rightkp=14;
leftkd=11;
rightkd=9;
system_mode=100;
break;
case 100:
if(t<3000){t++; //wait 3 sec
system_mode=100;
}
else{
{system_mode=1;
FTM_PWM_Duty(FTM1,CH1,3800);
}}
break;
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
case 8:
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
case 15:
case 16:
case 17:
case 18:
case 19:
case 20:
//get sensor advalue
for(i = 0; i < 6; i++){
ADvalue_left += ad_max(ADC0,SE14,ADC_16bit,20);
ADvalue_right += ad_max(ADC0,SE15,ADC_16bit,20);}
ADvalue_left=ADvalue_left/6;
ADvalue_right=ADvalue_right/6;
//decide turn left or turn right
ADvalue_left=ADvalue_left-41500;
ADvalue_right=ADvalue_right-42000;
if(ADvalue_left<0){ADvalue_left=1;} //avoid 0/0
if(ADvalue_right<0){ADvalue_right=1;}
k=ADvalue_right-ADvalue_left; // easy to go straight
if(k<1400 && k>-1400 && ADvalue_left>6000 && ADvalue_right>6000)
{k=750;
FTM_PWM_Duty(FTM0,CH1,k);
tempk=k;
system_mode=system_mode+1;
ADvalue_left=0; // set ad value to zero
ADvalue_right=0;
break;
}
q=ADvalue_right+ADvalue_left;
k=k*1000;
k=k/q;
if(k>0){
k=k*k;}
if(k<0){
k=-k*k;}
k=k/1000;
k=k*trankp/100;
if(k>0){k=k*rightkp/10;}
if(k<0){k=k*leftkp/10;}
k=750+k;
if(k>910){k=909;}
if(k<570){k=571;}
errork=k-tempk;
if(k>0){errork=errork*rightkd/10;}
if(k<0){errork=errork*leftkd/10;}
k=k+errork;
if(k>910){k=909;}
if(k<570){k=571;}
absoluterrork=k-tempk;
if(absoluterrork>50 || absoluterrork<-57){k=tempk;}
FTM_PWM_Duty(FTM0,CH1,k);
ADvalue_left=0;
ADvalue_right=0;
tempk=k;
system_mode=system_mode+1;//go to next state on next cycle
if(s<5000){s++;
rsw=0;}
else{
if(rsw>1){system_mode=22;}}
break;
case 21: // speed control
encoder20ms = FTM2_CNT;
error = ideal_speed-encoder20ms ;
if(error==ideal_speed){error= error*7;}
if(error>ideal_speed*7/10){error = error*4;}
if(error>ideal_speed*9/20){error = error*2;}
if(error>ideal_speed*1/5){error = error*1;}
if(error>0){error = error*1/5;}
if(error<-ideal_speed){error = error*7;}
if(error<-ideal_speed*7/10){error = error*4;}
if(error<-ideal_speed*9/20){error = error*2;}
if(error<-ideal_speed*1/5){error = error*1;}
if(error<0){error = error*1/5;}
if(encoder20ms!=ideal_speed){final_speed = final_speed+error;}
if(final_speed>8000){final_speed=7600;}
if( encoder20ms>ideal_speed*2)
{
gpio_set(PORTD,9,0);
final_speed= 2000;
}
else{gpio_set(PORTD,9,1);}
FTM_PWM_Duty(FTM1,CH1,final_speed);
FTM2_CNT=0;
system_mode=1; //back to the top of pit
if(s<5000){s++;
rsw=0;}
else{
if(rsw>1){system_mode=22;}}
break;
case 22:
FTM_PWM_init(FTM1,CH1,10000,0);
break;
}
PIT_Flag_Clear(PIT3);
}
void Speed_Calculate( float angle, float Gyro_Now)
{
PWM_angle = angle_now * P_ANGLE + Gyro_Now * D_ANGLE ;//angle最终融合角度 Rate融合角速度
speed_Start=PWM_angle+SpeedControlOut;//-fabss(Gyro_DIR);
Speed_L = speed_Start+DirectionOut;//g_fDirectionOut
Speed_R = speed_Start-DirectionOut;
/************************************/
if(Speed_L > 9900) Speed_L = 9900;
if(Speed_L < -9900) Speed_L = -9900;
if(Speed_R > 9900) Speed_R = 9900;
if(Speed_R < -9900) Speed_R = -9900;
/*********************************/
if(Speed_L > 0)
Speed_L_Last = 10000 - Speed_L;
else
Speed_L_Last = -10000 - Speed_L;
if(Speed_R > 0)
Speed_R_Last = 10000 - Speed_R;
else
Speed_R_Last = -10000 - Speed_R;
/***********PWM控制******************/
if(Speed_L >= 0)
{
FTM_PWM_Duty(FTM0, CH0, 10000);
FTM_PWM_Duty(FTM0, CH1,Speed_L_Last - MOTOR_DEAD_VAL_L); //??????
}
else
{
FTM_PWM_Duty(FTM0, CH1,10000);
FTM_PWM_Duty(FTM0, CH0,-Speed_L_Last - MOTOR_DEAD_VAL_L); //??????
}
if(Speed_R >= 0)
{
FTM_PWM_Duty(FTM0, CH2,10000);
FTM_PWM_Duty(FTM0, CH3,Speed_R_Last - MOTOR_DEAD_VAL_R); //??????
}
else
{
FTM_PWM_Duty(FTM0, CH3,10000);
FTM_PWM_Duty(FTM0, CH2,-Speed_R_Last - MOTOR_DEAD_VAL_R); //??????
}
if(angle_now>33||angle_now<-11)
{FTM_PWM_Duty(FTM0, CH0, 10000);
FTM_PWM_Duty(FTM0, CH1, 10000);
FTM_PWM_Duty(FTM0, CH2, 10000);
FTM_PWM_Duty(FTM0, CH3, 10000);
DisableInterrupts;}
}
void main()
{
uart_init(UART3, 115200); // For our flashed bluetooth
//uart_init(UART3, 9600); // For our non-flashed bluetooth
gpio_init(PORTE,6,GPI,0); // SW2 goes into gyro calibration mode
printf("\nWelcome to the SmartCar 2013 Sensor team developement system\n");
while(1){
printf("==============================================================\n");
printf("Please select mode:\n---------------------------\n");
printf("1:Accelerometer&gyro\n");
printf("2:LinearCCD\n");
printf("3:Flash Memory\n");
printf("4:Encoder testing\n");
printf("6:Motor control test\n");
printf("7:SystemLoop Test\n");
delayms(300);
if(gpio_get(PORTE,6)==0){
adc_init(ADC1,AD5b);
adc_init(ADC1,AD7b);
printf("We are now in gyro calibration mode, Please keep car stationery and wait till light dies");
gpio_init(PORTE,24,GPO,0);
gpio_init(PORTE,25,GPO,0);
gpio_init(PORTE,26,GPO,0);
gpio_init(PORTE,27,GPO,0);
delayms(3000);
turn_gyro_offset=ad_ave(ADC1,AD5b,ADC_12bit,1000);
balance_gyro_offset=ad_ave(ADC1,AD7b,ADC_12bit,1000);
store_u32_to_flashmem1(turn_gyro_offset);
store_u32_to_flashmem2(balance_gyro_offset);
gpio_turn(PORTE,24);
gpio_turn(PORTE,25);
gpio_turn(PORTE,26);
gpio_turn(PORTE,27);
while(1){}
}
g_char_mode = '7'; // Hard code mode = system loop
//g_char_mode = uart_getchar(UART3);
switch (g_char_mode){
case '0':
//VR analog input
adc_init(ADC0,AD14);
while(1){
delayms(500);
printf("\n%d",ad_once(ADC0,AD14,ADC_16bit));//vr value
}
break;
case '1':
uart_sendStr(UART3,"The mode now is 1: Accelerometer and Gyroscope Test");
//accl_init();
adc_init(ADC1,AD6b);
adc_init(ADC1,AD7b);
adc_init(ADC0,AD14);
adc_init(ADC1,AD4b);
balance_centerpoint_set=ad_ave(ADC0,AD14,ADC_12bit,10);
printf("\nEverything Initialized alright\n");
while(1)
{
//printf("\n\f====================================");
//control_tilt=(ad_ave(ADC1,AD6b,ADC_12bit,8)-3300)+(balance_centerpoint_set/10);
//printf("\nMain gyro%d",control_tilt);//theta
//printf("\n%d",ad_once(ADC1,AD7b,ADC_12bit)-1940);//omega
printf("\n%d",ad_ave(ADC1,AD6b,ADC_12bit,8)-940);
delayms(50);
}
break;
case '2':
uart_sendStr(UART3,"The mode now is 2: Linear CCD");
ccd_interrupts_init();
printf("\nEverything Initialized alright\n");
while(1)
{
//ccd_sampling(1); // Longer SI CCD Sampling
}
break;
case '3':
uart_sendStr(UART3,"The mode now is 3:Flash Memory\n");
Flash_init();
printf("Flash Memory init ok\n");
printf("writing the u32 : 3125 to memory 1\n");
store_u32_to_flashmem1(3125);
printf("writing the u32: 1019 to memory 2\n");
store_u32_to_flashmem2(1019);
printf("Now reading from memory\n");
printf("Memory 1:%d\n",get_u32_from_flashmem1());
printf("Memory 2:%d\n",get_u32_from_flashmem2());
while(1)
{
//stops loop to see results
}
break;
case '4':
//temporary case for debuggine encoder libraries, move to encoder.h later
uart_sendStr(UART3,"The mode now is 4: encoder test");
DisableInterrupts;
exti_init(PORTA,6,rising_up); //inits left encoder interrupt capture
exti_init(PORTA,7,rising_up); //inits right encoder interrupt capture
pit_init_ms(PIT1,500); //periodic interrupt every 500ms
EnableInterrupts;
printf("\nEverything Initialized alright\n");
while(1){
}
break;
case '6':
uart_sendStr(UART3,"The mode now is 6: Motor Control test");
//inits
motor_init();
printf("\nEverything Initialized alright\n");
delayms(1000);
while(1)
{
//printf("\n\fInput 0-9 Motor Speed, Currently:%d",motor_test);
//motor_test=100*(uart_getchar(UART3)-48);
FTM_PWM_Duty(FTM1,CH0,2000);
FTM_PWM_Duty(FTM1,CH1,2000);//left
printf("\n\f Input direction : 0 or 1");
motor_test = uart_getchar(UART3)-48;
if (motor_test){
gpio_set(PORTD,9,1);
gpio_set(PORTD,7,1);//this is DIR
}else{
gpio_set(PORTD,9,0);
gpio_set(PORTD,1,0);//this is DIR
}
}
break;
case '7':
printf("\n The Mode is now 7: SystemLoop Test");
adc_init(ADC1,AD6b);
adc_init(ADC1,AD7b);
adc_init(ADC0,AD14);
adc_init(ADC1,AD5b);
balance_centerpoint_set=ad_ave(ADC0,AD14,ADC_12bit,10);
motor_init();
gpio_set(PORTD,9,0); //dir
gpio_set(PORTD,7,0); //dir
FTM_PWM_Duty(FTM1, CH0, 3000); //initital speed
FTM_PWM_Duty(FTM1, CH1, 3000); //initital speed
ccd_interrupts_init();
pit_init_ms(PIT3,1);
printf("\nEverything inited alright");
while(1){
//system loop runs
}
break;
default :
printf("\n\fYou entered:%c, Please enter a number from 1-7 to select a mode\n\f",g_char_mode);
}
}
}
void Steering_Change()
{
if(flag) LDC_SPI1_val += adjustment[0];
else LDC_SPI0_val += adjustment[0];
LDC_result = LDC_SPI0_val - LDC_SPI1_val;
if(LDC_SPI0_val > adjustment[6]+adjustment[0]-60 && LDC_SPI1_val > adjustment[6]+adjustment[0]-60)
{
Set_SteeringPD(0.1200,0.00);
}
else
{
if(LDC_result > 0)
{
//Set_SteeringPD(0.1200,0.1553);
divisor_end = divisor2;
}
else
{
//Set_SteeringPD(0.1200,0.2813);
divisor_end = divisor;
}
}
first_Steering_Error = LDC_result;
p_Steering_Value = kp_Steering * first_Steering_Error;
d_Steering_Value = (first_Steering_Error - second_Steering_Error) * kd_Steering;
caculate_Steering_Value = p_Steering_Value + d_Steering_Value;
LDC_result = caculate_Steering_Value / divisor_end;
// switch(state)
// {
// case 0:
//// if(LDC_result > ANGLE_LEFT_MIN && LDC_result < ANGLE_RIGHT_MIN)
//// {
//// //AimSpeed = 300.0;
//// }
//// else state = 1;
//
// if(LDC_result > ANGLE_LEFT_MIN && LDC_result < ANGLE_RIGHT_MIN)
// state = 0;
// if(LDC_result <= ANGLE_LEFT_MIN) state = 2;
// if(LDC_result >= ANGLE_RIGHT_MIN) state = 3;
// //AimSpeed = 300.0;
//
// break;
// case 1:
// if(LDC_result > ANGLE_LEFT_MIN && LDC_result < ANGLE_RIGHT_MIN)
// state = 0;
// if(LDC_result <= ANGLE_LEFT_MIN) state = 2;
// if(LDC_result >= ANGLE_RIGHT_MIN) state = 3;
// //AimSpeed = 300.0;
// break;
// case 2:
// if(LDC_result > ANGLE_LEFT_MIN && LDC_result <ANGLE_RIGHT_MIN)
// state = 0;
// //AimSpeed = 400.0;
// Left_Out_Side();
// break;
// case 3:
// if(LDC_result > ANGLE_LEFT_MIN && LDC_result <ANGLE_RIGHT_MIN)
// state = 0;
// //AimSpeed = 300.0;
// Right_Out_Side();
// //Set_SteeringPD(0.1254,0.3689);
// break;
// }
Left_Out_Side();
Right_Out_Side();
if(LDC_result >= STEERING_ANGLE_MAX) LDC_result = STEERING_ANGLE_MAX;
else if(LDC_result <= STEERING_ANGLE_MIN) LDC_result = STEERING_ANGLE_MIN;
FTM_PWM_Duty(FTM1,FTM_CH0,angle_to_period(LDC_result+1));
//LCD_BL(0,0,(uint16)Abs(state));
//LCD_BL(30,0,(uint16)Abs(LDC_result));
result_pre = LDC_result;
LDC_SPI0_pre = LDC_SPI0_val;
LDC_SPI1_pre = LDC_SPI1_val;
}
void Motor_stop()
{
FTM_PWM_Duty(FTM0,FTM_CH2,0);
FTM_PWM_Duty(FTM0,FTM_CH1,0);
}
void motor_control()
{
float control_l;
float control_r;
unsigned int motor_control_l=0;
unsigned int motor_control_r=0;
control_l=balance_out_l+direction_out_l+speed_out_data; //左轮控制量叠加
control_r=balance_out_r+direction_out_r+speed_out_data; //右轮控制量叠加
control_l=1.0*control_l;
// if((acc_z_juedui>3000.0)||(acc_z_juedui<1000.0)) //车倒保护 2430 1540
// {
// control_l=0;
// control_r=0;
// }
if(control_l>980.0)
{
control_l=980.0;
}
if(control_r>980.0)
{
control_r=980.0;
}
if(control_l<-980.0)
{
control_l=-980.0;
}
if(control_r<-980.0)
{
control_r=-980.0;
}
if(control_l>=0.0) //左轮前进 //PWM输出
{
motor_control_l=(unsigned int)control_l; //38 //强制转换为整型，下一步待用
gpio_set (PORTC,4,0); //PWM_R1 c1 左轮
FTM_PWM_Duty(FTM0, CH2, motor_control_l); //PWM_R2 C2
}
if(control_r>=0.0) //右轮前进
{
motor_control_r=(unsigned int)control_r; //23 //强制转换为整型，下一步待用
gpio_set (PORTC,1,0); //PWM_R1 c1
FTM_PWM_Duty(FTM0, CH1, motor_control_r); //PWM_R2 C2
}
if(control_l<0.0) //左轮后退
{
motor_control_l=(unsigned int) (1000+control_l); //-38
gpio_set (PORTC,4,1); //PWM_R1 c1
FTM_PWM_Duty(FTM0, CH2, motor_control_l); //PWM_R2 C2
}
if(control_r<0.0) //右轮后退
{
motor_control_r=(unsigned int)(1000+control_r); //-23
gpio_set (PORTC,1,1); //PWM_R1 c1
FTM_PWM_Duty(FTM0, CH1, motor_control_r); //PWM_R2 C2
}
balance_out_l=0.0;
balance_out_r=0.0;
//direction_out_l=0.0;
//direction_out_r=0.0;
control_l=0.0;
control_r=0.0;
motor_control_l=0;
motor_control_r=0;
}
void PIT0_IRQHandler(void)
{
DisableInterrupts;
PIT_Flag_Clear(PIT0);
zlf++;
sf++;
df++;
if(b==1)
{
if(fanzhuan==0)
{
sdsd=700;//1350
fanzhuan=1;
}
}
if(d==1||g_fCarAngle>=35)
while(1)
{
FTM_PWM_Duty(FTM0, CH0, 0);
FTM_PWM_Duty(FTM0, CH1, 0);
FTM_PWM_Duty(FTM0, CH2, 0);
FTM_PWM_Duty(FTM0, CH3, 0);
}
/////////////////////////////////////////////////////分段
g_nSpeedControlPeriod++;
speedcontrolout();
g_nDirectionControlPeriod++;
directcontrolout();
//////////////////////////////////////////////
if(zlf>=5)
{
zlf=0;
AD_Calculate();
Speed_Calculate(g_fCarAngle,Gyro_Now);//清华
// Speed_Calculate(angle,angle_dot);//卡尔曼
ssc();///////////////////////////////////////////////////PWM驱动
}
if(df>=10)
{
df=0;
g_nDirectionControlPeriod=0;
directcontrol(Find_Mid_Place);///////////////////////////////////传入中线值
}
if(sf>=100)
{
sf=0;
g_nSpeedControlPeriod=0;
speedcontrol();
}
if(js==0)
{
StartIntegration1();
StartIntegration2();
js=1;
}
else
fs++;
EnableInterrupts;
}
