void InitPinModes(void){
BYTE i;
BYTE mode=0;
for (i=0;i<24;i++){
//ClearPinState(i);
SetPinTris(i,INPUT);
SetDIO(i,0);
SetPWM(i,EEReadValue(i));
SetServoPos(i,EEReadValue(i),0);
//getBcsIoDataTable()[i].PIN.previousChannelMode=NO_CHANGE;
mode=EEReadMode(i);
if((mode < 2)||(mode >=IO_MODE_MAX)){
EEWriteMode(i,IS_DI);
mode = EEReadMode(i);
}
getBcsIoDataTable()[i].PIN.currentChannelMode = mode;
setMode(i,EEReadMode(i));
}
startup = FALSE;
//printModes();
}
BOOL SetChanVal(BYTE pin,INT32 bval, float time){
BYTE mode = GetChannelMode(pin);
//println_I("Setting channel pos\n\tchan: ");p_int_I(pin);print_I(" \n\tto val: ");p_int_I(bval);print_I("\n\tin time: ");p_fl_I(time);
switch (mode){
case IS_DI:
case IS_DO:
SetDIO(pin,bval);
break;
case IS_SERVO:
SetServoPos(pin,bval,time);
break;
case IS_PWM:
SetPWM(pin,bval);
break;
case IS_DC_MOTOR_VEL:
case IS_DC_MOTOR_DIR:
SetDCMotor(pin,bval);
break;
default:
return FALSE;
}
return TRUE;
}
void Moto_PwmRflash(long MOTO1_PWM,long MOTO2_PWM, long MOTO3_PWM, long MOTO4_PWM)
{
/*if(MOTO1_PWM>Moto_PwmMax) MOTO1_PWM = Moto_PwmMax;
if(MOTO2_PWM>Moto_PwmMax) MOTO2_PWM = Moto_PwmMax;
if(MOTO3_PWM>Moto_PwmMax) MOTO3_PWM = Moto_PwmMax;
if(MOTO4_PWM>Moto_PwmMax) MOTO4_PWM = Moto_PwmMax;
if(MOTO1_PWM<0) MOTO1_PWM = 0;
if(MOTO2_PWM<0) MOTO2_PWM = 0;
if(MOTO3_PWM<0) MOTO3_PWM = 0;
if(MOTO4_PWM<0) MOTO4_PWM = 0;*/
//Êä³öPWMµÄÖµ
PWM_0_Width += MOTO1_PWM;
PWM_1_Width += MOTO2_PWM;
PWM_2_Width += MOTO3_PWM;
PWM_3_Width += MOTO4_PWM;
if (PWM_0_Width > 4800)
PWM_0_Width = 4800;
if (PWM_1_Width > 4800)
PWM_1_Width = 4800;
if (PWM_2_Width > 4800)
PWM_2_Width = 4800;
if (PWM_3_Width > 4800)
PWM_3_Width = 4800;
if (PWM_0_Width < 3200)
PWM_0_Width = 3200;
if (PWM_1_Width < 3200)
PWM_1_Width = 3200;
if (PWM_2_Width < 3200)
PWM_2_Width = 3200;
if (PWM_3_Width < 3200)
PWM_3_Width = 3200;
//DisEnablePWM();
SetPWM();
//EnablePWM();
}
void InitLcdController (void)
{
TIM_TIMERCFG_Type TIM_ConfigStruct;
uint8_t i2c_buf[2];
/* Transmit setup */
I2C_M_SETUP_Type txsetup;
TIM_ConfigStruct.PrescaleOption = TIM_PRESCALE_USVAL;
TIM_ConfigStruct.PrescaleValue = 1;
// Set configuration for Tim_config and Tim_MatchConfig
TIM_Init(LPC_TIM0, TIM_TIMER_MODE,&TIM_ConfigStruct);
init_i2c(100000);
txsetup.sl_addr7bit = I2C_PCA9532_ADDR;
txsetup.tx_data = i2c_buf;
txsetup.tx_length = 2;
txsetup.rx_data = NULL;
txsetup.rx_length = 0;
txsetup.retransmissions_max = 3;
// "v1,cc0,c31,d50,o,d200,c51,cc100";
// 1st letter 2nd letter Meaning
// c c Send command to update PWM
// d Send command to update the status for Display Enable Pin
// 3 Send command to update the status for 3V3 pin
// 5 Send command to update the status for 5V pin
// d Number Delay in a number of miliseconds
// o open the LCD
// v Sequence version info
// PWM setting
SetPWM(0);
// 3V3 pin
i2c_buf[0] = LSn(LCD_3V3_PIN_NUM);
txsetup.tx_length = 1;
txsetup.rx_data = &i2c_buf[1];
txsetup.rx_length = 1;
if (I2C_MasterTransferData((en_I2C_unitId)I2CDEV, &txsetup, I2C_TRANSFER_POLLING) != SUCCESS){
return;
}
i2c_buf[1]&= ~0x03<<(BITn(LCD_3V3_PIN_NUM));
i2c_buf[1]|= LCD_OUT_LED<<(BITn(LCD_3V3_PIN_NUM));
txsetup.tx_length = 2;
txsetup.rx_data = NULL;
txsetup.rx_length = 0;
if (I2C_MasterTransferData((en_I2C_unitId)I2CDEV, &txsetup, I2C_TRANSFER_POLLING) != SUCCESS){
return;
}
TIM_Waitms(250);
// 5V pin
i2c_buf[0] = LSn(LCD_5V_PIN_NUM);
txsetup.tx_length = 1;
txsetup.rx_data = &i2c_buf[1];
txsetup.rx_length = 1;
if (I2C_MasterTransferData((en_I2C_unitId)I2CDEV, &txsetup, I2C_TRANSFER_POLLING) != SUCCESS){
return;
}
i2c_buf[1]&= ~(0x03 << (BITn(LCD_5V_PIN_NUM)));
i2c_buf[1]|= LCD_OUT_LED <<(BITn(LCD_5V_PIN_NUM));
txsetup.tx_length = 2;
txsetup.rx_data = NULL;
txsetup.rx_length = 0;
if (I2C_MasterTransferData((en_I2C_unitId)I2CDEV, &txsetup, I2C_TRANSFER_POLLING) != SUCCESS){
return;
}
// Set PWM
SetPWM(100);
TIM_DeInit(LPC_TIM0);
}
// This function sets a motor speed
static void SetTLEMotor(tTLEMotor *mtr, float input) {
// Check the input range
if (input > 1 || input < -1)
return;
// invert if set
if (mtr->invert)
input *= -1;
// Operate the motor controller
// Motor controller operation is specific
// to the TLE5205-2
if (mtr->brake) {
if (input < 0) {
// CCW (P, ~P)
SetPWM(mtr->pwm0, 1.0f+input, 0.0f);
SetPWM(mtr->pwm1, -input, 1.0f+input);
} else if (input > 0) {
// CW (P, 0)
SetPWM(mtr->pwm0, 1.0f-input, 0.0f);
SetPWM(mtr->pwm1, 0.0f, 0.0f);
} else {
// S (1, 0)
SetPWM(mtr->pwm0, 1.0f, 0.0f);
SetPWM(mtr->pwm1, 0.0f, 0.0f);
}
} else {
if (input < 0) {
// CCW (P, 1)
SetPWM(mtr->pwm0, 1.0f+input, 0.0f);
SetPWM(mtr->pwm1, 1.0f, 0.0f);
} else if (input > 0) {
// CW (P, P)
SetPWM(mtr->pwm0, 1.0f-input, 0.0f);
SetPWM(mtr->pwm1, 1.0f-input, 0.0f);
} else {
// S (1, 1)
SetPWM(mtr->pwm0, 1.0f, 0.0f);
SetPWM(mtr->pwm1, 1.0f, 0.0f);
}
}
}
int main (void)
{
//Test
//Test();
//Init
InitDevice();
//Send Startup Message
FCMessageSend("vFlyOK");
//等待INS的GPS完成启动
//WaitGPS();
//改变运行状态并发送状态信息
RunState=DefaultRunState;
FCEventSend(RunState);
//初始化参数
InitPara();
FCMessageSend("InitParaOK");
//启动主计时器
TimerEnable(0);
while(1)
{
if(Do==DoWorkLoop)
{
LED2ON();
INS();
SwitchMode();
if(FPGAState==TRUE)
{
GetPWM();
}
//FCA();
//FCALimitPWM(void);
FCAGetNav();
FCAPWMOut();
SmoothPWM(40);
SetPWM();
//CollectMedianDataFun(40);
FCACollectPosYaw(5);
FCACollectMedianDataFun(40);
FCFrameProcess();
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
MainLoopRunCounter++;
LED2OFF();
Do=DoNothing;
}
if(Do==DoFactoryLoop)
{
LED1ON();
GpioClrLOW(P1_25);//一律切换到手驾
INS();
FCFrameProcess();
if(DataToDownload==0)
{
if(MainLoopRunCounter%DataSendDF==0)
{
DataReturn(0);
}
/*
if(MainLoopRunCounter%DataRecDF==DataRecDF/2)
{
DataReturn(1);
}
*/
}
else
{
DataDownload();
}
MainLoopRunCounter++;
LED1OFF();
Do=DoNothing;
}
}
return 0;
}
void RxComdAction() {
if(MotorAddress == RxBuffer[0]) {
if(CheckCrcOk()) {
uint16_t cdat = (uint16_t)RxBuffer[2]*0x100+ RxBuffer[3];
switch(RxBuffer[1]) {
//红LED命令
case 1:
if(cdat == 1) LEDRED_ON;
if(cdat == 0) LEDRED_OFF;
break;
//绿LED命令
case 2:
if(cdat == 1) LEDGRN_ON;
if(cdat == 0) LEDGRN_OFF;
break;
//A+管脚（仅调试用）
case 3:
if(cdat == 1) POS_A_ON;
if(cdat == 2) PWM_A_ON;
break;
//B+管脚（仅调试用）
case 4:
if(cdat == 1) POS_B_ON;
if(cdat == 2) PWM_B_ON;
break;
//C+管脚（仅调试用）
case 5:
if(cdat == 1) POS_C_ON;
if(cdat == 2) PWM_C_ON;
break;
//A-管脚（仅调试用）
case 6:
switch(cdat) {
case 0: break;
case 1: NEG_A_ON; break;
case 2: MUTEX_NEG_A_ON; break;
}
break;
//B-管脚（仅调试用）
case 7:
switch(cdat) {
case 0: break;
case 1: NEG_B_ON; break;
case 2: MUTEX_NEG_B_ON; break;
}
break;
//C-管脚（仅调试用）
case 8:
switch(cdat) {
case 0: break;
case 1: NEG_C_ON; break;
case 2: MUTEX_NEG_C_ON; break;
}
break;
//FETS（仅调试用）
case 9:
switch(cdat) {
case 0: FETS_OFF; break;
case 1: POS_ABC_OFF; break;
case 2: NEG_ABC_OFF; break;
case 3: PWM_OFF; break;
case 4: ALLPIN_AND_PWM_OFF; break;
}
break;
//PWM设置（仅调试用）
case 10:
SetPWM((uint8_t)cdat);
break;
//电机仅启动命令（仅调试用）
case 11:
MotorStartUp(cdat);
ALLPIN_AND_PWM_OFF;
PWM = 0; SetPWM(PWM);
break;
//电机正常PWM启动命令
case 12:
PwmSet = 1;
PwmSetValue = cdat;
break;
//其他
default:
break;
} //end of Switch
} //end of CrcCheck
} //end of RxBuffer[0]
//清零接收缓冲区指针
rx_ptr = 0;
}
/*
SetPWM(2, 0);
SetPWM(3, 255);
Wait(3000);
SetPWM(2, 127);
SetPWM(3, 127);
*/
void driveStraight(void)
{
SetPWM(2, 0);
SetPWM(3, 255);
}
int main(int argc, char **argv)
{
int i;
char ts[50]; //timestring
int cc; //current counter (9S08QG8)
int hc[4]; //hour counter (9S08QG8)
char tv[2]; //temperatur value (LM75)
if(argc == 2){
if(strncmp(argv[1],"-V",2) == 0){
verbose_max = true;
verbose = true;
}else if(strncmp(argv[1],"-v",2) == 0){
verbose = true;
}else if(strncmp(argv[1],"-r",2) == 0){
readtest = true;
}else if(strncmp(argv[1],"-w",2) == 0){
writetest = true;
}else if(strncmp(argv[1],"-c",2) == 0){
verbose = true;
calibrate_9s08 = true;
}else if(strncmp(argv[1],"-d",2) == 0){
ValueToDatabase(false,"2015-07-23 00:00:00",1,2,3,10,11,12,13);
}else if(strncmp(argv[1],"-t",2) == 0){
verbose = true;
ThreadPowerOff();
return(0);
}else if(strncmp(argv[1],"-T",2) == 0){
//verbose_max = true;
verbose = true;
restoredata_9S08 = true;
}
}else if(argc == 4){
if(strncmp(argv[1],"-p",2) == 0){
printf("v1: %s\n",argv[1]);
printf("v2: %s\n",argv[2]);
printf("v3: %s\n",argv[3]);
if(strncmp(argv[2],"1",1) == 0){
int val = atoi(argv[3]);
printf("val: %d\n",val);
SetPWM(1,val);
}else if(strncmp(argv[2],"2",1) == 0){
int val = atoi(argv[3]);
SetPWM(2,val);
}
return(0);
}
}
if(readtest){
ReadTest();
return(0);
}
if(writetest){
WriteTest();
return(0);
}
if(calibrate_9s08){
Calibrate9s08();
return(0);
}
if(restoredata_9S08){
BackupValueFromDatabase(&cc,&hc[0],&hc[1],&hc[2],&hc[3]);
SetCounterValue_MC9S08QG8(cc);
if(verbose){
printf("counter: %d\n",cc);
}
for(i=0;i<4;i++){
SetHourCounterValue_MC9S08QG8(i,hc[i]);
if(verbose){
printf("hourcounter [%d]: %d\n",i,hc[i]);
}
}
return(0);
}
strcpy(ts,GetDateTimeString());
if(verbose){
printf("Uhrzeit: %s\n",ts);
}
cc = GetCounterValue_MC9S08QG8();
if(verbose){
printf("counter: %d\n",cc);
}
for(i=0;i<4;i++){
hc[i] = GetHourCounterValue_MC9S08QG8(i);
if(verbose){
printf("hourcounter [%d]: %d\n",i,hc[i]);
}
}
for(i=0;i<2;i++){
tv[i] = GetTemperaturValue_LM75_8Bit(i);
if(verbose){
printf("LM75.%d temperature: %d\n",i,tv[i]);
}
}
ValueToDatabase(false,ts,cc,tv[0],tv[1],hc[0],hc[1],hc[2],hc[3]);
return(0);
}
void rotateRight(void)
{
SetPWM(2, 255);
SetPWM(3, 255);
}
void stop(void)
{
SetPWM(2, 127);
SetPWM(3, 127);
}
/*********************************************************************
*
* Dialog callback routine
*/
static void _cbBk_Frame(WM_MESSAGE * pMsg)
{
int NCode, Id;
WM_HWIN hWin = pMsg->hWin;
switch (pMsg->MsgId)
{
case WM_PAINT:
PaintDialog(pMsg);
break;
case WM_INIT_DIALOG:
InitDialog(pMsg);
break;
case WM_KEY:
switch (((WM_KEY_INFO*)(pMsg->Data.p))->Key)
{
case GUI_KEY_ESCAPE:
GUI_EndDialog(hWin, 1);
break;
case GUI_KEY_ENTER:
GUI_EndDialog(hWin, 0);
break;
}
break;
case WM_NOTIFY_PARENT:
GUI_DispStringAt("Frame",10,220);
Id = WM_GetId(pMsg->hWinSrc);
NCode = pMsg->Data.v;
switch(NCode)
{
case WM_NOTIFICATION_RELEASED:
{
if(Id == GUI_ID_BUTTON0)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'0');
}
if(Id == GUI_ID_BUTTON1)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'1');
}
if(Id == GUI_ID_BUTTON2)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'2');
}
if(Id == GUI_ID_BUTTON3)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'3');
}
if(Id == GUI_ID_BUTTON4)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'4');
}
if(Id == GUI_ID_BUTTON5)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'5');
}
if(Id == GUI_ID_BUTTON6)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'6');
}
if(Id == GUI_ID_BUTTON7)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'7');
}
if(Id == GUI_ID_BUTTON8)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'8');
}
if(Id == GUI_ID_BUTTON9)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'9');
}
if(Id == GUI_ID_BUTTON10)
{
EDIT_AddKey(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT),'.');
}
/* BackSpace */
if(Id == GUI_ID_BUTTON11)
{
//EDIT_SetText(WM_GetDialogItem(hWin,GUI_ID_EDIT0),NULL);//清空输入框，挨个删除用下面两行
WM_SetFocus(WM_GetDialogItem(hWN_Dialog_PID_Frame,PID_EDIT));
GUI_SendKeyMsg(GUI_KEY_BACKSPACE,1);
GPIO_ResetBits(GPIOF,GPIO_Pin_7);
}
/* PID Parameter Edit choese*/
{
if(Id == GUI_ID_BUTTON12 || Id == GUI_ID_EDIT0)
{
PID_Para_Edit_Flag = Kp;
PID_EDIT = PID_Para_Edit_Flag + GUI_ID_EDIT0;
}
if(Id == GUI_ID_BUTTON13 || Id == GUI_ID_EDIT1)
{
PID_Para_Edit_Flag = Ki;
PID_EDIT = PID_Para_Edit_Flag + GUI_ID_EDIT0;
}
if(Id == GUI_ID_BUTTON14 || Id == GUI_ID_EDIT2)
{
PID_Para_Edit_Flag = Kd;
PID_EDIT = PID_Para_Edit_Flag + GUI_ID_EDIT0; //GUI_ID_EDIT2
}
if(Id == GUI_ID_BUTTON15 || Id == GUI_ID_EDIT3)
{
PID_Para_Edit_Flag = E;
PID_EDIT = PID_Para_Edit_Flag + GUI_ID_EDIT0;
}
if(Id == GUI_ID_BUTTON17 || Id == GUI_ID_EDIT4)
{
PID_Para_Edit_Flag = Set;
PID_EDIT = PID_Para_Edit_Flag + GUI_ID_EDIT0; //GUI_ID_EDIT2
}
/* OK Key */
if(Id == GUI_ID_BUTTON16)
{
/* Store the last array(5) */
value_PID[CounterPID].Proportion = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT0) );
value_PID[CounterPID].Integral = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT1) );
value_PID[CounterPID].Derivative = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT2) );
value_PID[CounterPID].SetPoint = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT4) );
BK_Light = SLIDER_GetValue(WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_SLIDER0));
SetPWM(BK_Light,PWM_BACK_LIGHT);
//!< Not the PID configuration Period
PID_CFG_Flag = 0;
WM_DeleteWindow(hWN_Dialog_PID_Frame);
WM_ShowWindow(hWM_HBKWIN_CURVE);
WM_HideWindow(hWM_HBKWIN_CFG_PID);
}
}
/* Up and down of the order of the PID Array */
{
if(Id == GUI_ID_BUTTON18)
{
value_PID[CounterPID].Proportion = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT0) );
value_PID[CounterPID].Integral = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT1) );
value_PID[CounterPID].Derivative = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT2) );
value_PID[CounterPID].SetPoint = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT4) );
/* Get the E_Percent */
{
if(0 == CounterPID)
{
E_Percent[0] = EDIT_GetFloatValue ( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT3) );
E_Percent[1] = 1 - E_Percent[0];
}
}
CounterPID++;
if(6 == CounterPID)
{
CounterPID = 0;
}
itoa(CounterPID,bufTextOrder,10);
TEXT_SetText( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_TEXT0),bufTextOrder);
if(1 == CounterPID)
{
EDIT_SetFloatValue ( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT3), E_Percent[1]);
}
/* Set the same value to edit */
if( 2 == CounterPID || 4 == CounterPID)
{
EDIT_SetFloatValue(WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT3),E_Percent[0]);
EDIT_SetFloatValue(WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT4),value_PID[0].SetPoint);
}
else if( 3 == CounterPID || 5 == CounterPID)
{
EDIT_SetFloatValue(WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT3),E_Percent[1]);
EDIT_SetFloatValue(WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT4),value_PID[1].SetPoint);
}
}
/** Previous button **/
if(Id == GUI_ID_BUTTON19)
{
value_PID[CounterPID].Proportion = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT0) );
value_PID[CounterPID].Integral = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT1) );
value_PID[CounterPID].Derivative = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT2) );
value_PID[CounterPID].SetPoint = EDIT_GetFloatValue( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_EDIT4) );
CounterPID--;
if(-1 == CounterPID)
{
CounterPID = 5;
}
itoa(CounterPID,bufTextOrder,10);
TEXT_SetText( WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_TEXT0),bufTextOrder);
}
}
if(Id == GUI_ID_SLIDER0)
{
BK_Light = SLIDER_GetValue(WM_GetDialogItem(hWN_Dialog_PID_Frame,GUI_ID_SLIDER0));
SetPWM(BK_Light,PWM_BACK_LIGHT);
}
}
break;
}
default:
WM_DefaultProc(pMsg);
}
}
//############################################################################
//Hauptprogramm
int main (void)
//############################################################################
{
char altPhase = 0;
int test = 0;
unsigned int Blink,TestschubTimer;
unsigned int Blink2,MittelstromTimer,DrehzahlMessTimer,MotorGestopptTimer;
DDRC = 0x08; //PC3输出，控制红色led 端口C 数据方向寄存器
PORTC = 0x08; //PC3输出高电平，红色led亮
DDRD = 0x3A; //10111010A-B-C-配置为输出，TXD,LED_GRN(PD7)配置为输出，MITTEL/RXD/INTO为输入
PORTD = 0x00;
DDRB = 0x0E;
PORTB = 0x31;
#if (MOTORADRESSE == 0)
PORTB |= (ADR1 + ADR2); // Pullups für Adresswahl
for(test=0;test<500;test++);
if(PINB & ADR1)
{
if (PINB & ADR2) MotorAdresse = 1;
else MotorAdresse = 2;
}
else
{
if (PINB & ADR2) MotorAdresse = 3;
else MotorAdresse = 4;
}
HwVersion = 11;
#else
MotorAdresse = MOTORADRESSE;
HwVersion = 10;
#endif
if(PIND & 0x80) {HwVersion = 12; IntRef = 0xc0;} //为1.2版本
DDRD = 0xBA;
UART_Init();
Timer0_Init();
sei();//Globale Interrupts Einschalten
// Am Blinken erkennt man die richtige Motoradresse
/*
for(test=0;test<5;test++)
{
if(test == MotorAdresse) PORTD |= GRUEN;
Delay_ms(150);
PORTD &= ~GRUEN;
Delay_ms(250);
}
Delay_ms(500);
*/
// UART_Init(); // war doppelt
PWM_Init();
InitIC2_Slave(0x50);
InitPPM();
Blink = SetDelay(101);
Blink2 = SetDelay(102);
MinUpmPulse = SetDelay(103);
MittelstromTimer = SetDelay(254);
DrehzahlMessTimer = SetDelay(1005);
TestschubTimer = SetDelay(1006);
while(!CheckDelay(MinUpmPulse))
{
if(SollwertErmittlung()) break; // 一旦发现有信号，立刻停止延时
}
GRN_ON;
PWM = 0;
SetPWM();
SFIOR = 0x08; // Analog Comperator ein
ADMUX = 1;
MinUpmPulse = SetDelay(10);
DebugOut.Analog[1] = 1; // 填写一些调试信号
PPM_Signal = 0;
if(!SollwertErmittlung()) MotorTon();
//MotorTon();
PORTB = 0x31; // Pullups wieder einschalten
// zum Test der Hardware; Motor dreht mit konstanter Drehzahl ohne Regelung
if(TEST_MANUELL) Anwerfen(TEST_MANUELL); // kommt von dort nicht wieder
while (1)
{
//ShowSense();
if(!TEST_SCHUB) PWM = SollwertErmittlung();
//I2C_TXBuffer = PWM; // Antwort über I2C-Bus
if(MANUELL_PWM) PWM = MANUELL_PWM;
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(Phase != altPhase) // es gab eine Kommutierung im Interrupt
{
MotorGestoppt = 0;
ZeitFuerBerechnungen = 0; // direkt nach einer Kommutierung ist Zeit
MinUpmPulse = SetDelay(250); // Timeout, falls ein Motor stehen bleibt
altPhase = Phase; // 旧相位 = 新相位
}
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!PWM) // Sollwert == 0 在开机后主控板静默,并没有发任何I2C信号过来
{
MotorAnwerfen = 0; // kein Startversuch MotorAnwerfen means motor start
ZeitFuerBerechnungen = 0; // 指使一些操作的优先级用
// nach 1,5 Sekunden den Motor als gestoppt betrachten
if(CheckDelay(MotorGestopptTimer))
{
DISABLE_SENSE_INT;
MotorGestoppt = 1;
STEUER_OFF;
}
}
else
{
if(MotorGestoppt) MotorAnwerfen = 1; // Startversuch
MotorGestopptTimer = SetDelay(1500);
}
if(MotorGestoppt && !TEST_SCHUB) PWM = 0; //TEST_SCHUB 1为测试模式，0为正常模式
SetPWM();
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(!ZeitFuerBerechnungen++) //先!,后++
{
if(MotorGestoppt) // 马达停止
{
GRN_ON;
FastADConvert();
}
if(SIO_DEBUG)
{
DebugAusgaben(); // welche Werte sollen angezeigt werden?
if(!UebertragungAbgeschlossen) SendUart();
else DatenUebertragung();
}
// Berechnen des Mittleren Stroms zur (langsamen) Strombegrenzung
if(CheckDelay(MittelstromTimer)) //254ms
{
MittelstromTimer = SetDelay(50); // alle 50ms
if(Mittelstrom < Strom) Mittelstrom++;// Mittelwert des Stroms bilden 将mittelstorm平缓化
else if(Mittelstrom > Strom) Mittelstrom--;
if(Strom > MAX_STROM) MaxPWM -= MaxPWM / 32; //为什么电流大于最大电流的时候，PWM最大值要减小？
if((Mittelstrom > LIMIT_STROM))// Strom am Limit? 动态调整MaxPWM的值
{
if(MaxPWM) MaxPWM--;// dann die Maximale PWM herunterfahren
PORTC |= ROT;
}
else
{
if(MaxPWM < MAX_PWM) MaxPWM++;
}
}
if(CheckDelay(DrehzahlMessTimer)) // Ist-Drehzahl bestimmen 1005ms
{
DrehzahlMessTimer = SetDelay(10);
SIO_Drehzahl = CntKommutierungen;//(6 * CntKommutierungen) / (POLANZAHL / 2);
CntKommutierungen = 0;
// if(PPM_Timeout == 0) // keine PPM-Signale
ZeitZumAdWandeln = 1;
}
#if TEST_SCHUB == 1
{
if(CheckDelay(TestschubTimer))
{
TestschubTimer = SetDelay(1500);
switch(test)
{
case 0: PWM = 50; test++; break;
case 1: PWM = 130; test++; break;
case 2: PWM = 60; test++; break;
case 3: PWM = 140; test++; break;
case 4: PWM = 150; test = 0; break;
default: test = 0;
}
}
}
#endif
// Motor Stehen geblieben
if((CheckDelay(MinUpmPulse) && SIO_Drehzahl == 0) || MotorAnwerfen) // SIO_Drehzahl 为当前转速值 前面为判断意外停机
{
/* 下面三行代码给电机意外停转用*/
MotorGestoppt = 1;
DISABLE_SENSE_INT; // 取消比较中断
MinUpmPulse = SetDelay(100);
if(MotorAnwerfen)
{
PORTC &= ~ROT; //switch off red led
Strom_max = 0;
MotorAnwerfen = 0;
if(Anwerfen(10)) // 以PMW = 10触发启动程序，期间要换相32次,成功返回1
{
GRN_ON; //绿灯打开
MotorGestoppt = 0; //
Phase--; // Phase = 1
PWM = 1;
SetPWM();
SENSE_TOGGLE_INT; // ?????
ENABLE_SENSE_INT; // ????? 使能比较器中断
MinUpmPulse = SetDelay(20);
while(!CheckDelay(MinUpmPulse)); // kurz Synchronisieren
PWM = 15;
SetPWM();
MinUpmPulse = SetDelay(300);
while(!CheckDelay(MinUpmPulse)) // kurz Durchstarten
{
if(Strom > LIMIT_STROM/2)
{
STEUER_OFF; // Abschalten wegen Kurzschluss
RotBlink(10);
MotorAnwerfen = 1;
}
}
// Drehzahlmessung wieder aufsetzen
DrehzahlMessTimer = SetDelay(50);
altPhase = 7;
}
else if(SollwertErmittlung()) MotorAnwerfen = 1;
}
}
} // ZeitFuerBerechnungen
} // while(1) - Hauptschleife
}
void Motor_set(Motor_Port port, Motor_Speed speed) {
SetPWM(port, ConvertUnsigned(speed));
// SetPWM(port, speed + 127);
}