//-------------------------------------------------------------------------------
void GoToRange_V2(){
bool enter_range_X = false,enter_range_Y = false;
DoTheAd(AD_AVGtime,0);
(Angle_X < 0) ? MotoControl_Run(1,CW) : MotoControl_Run(1,CCW);
(Angle_Y < 0) ? MotoControl_Run(2,CW) : MotoControl_Run(2,CCW);
long int countTTT = 0;
while(1){
DoTheAd(AD_AVGtime,0);
countTTT++;
if( ( (abs(Angle_X)) < 1) && !enter_range_X ){
enter_range_X = true;
MotoControl_Stop(1);
sprintf(string," X Into Range = %32f \r\n",Angle_X); UART1_SendStr(string);
}
if( ( (abs(Angle_Y)) < 1) && !enter_range_Y ){
enter_range_Y = true;
MotoControl_Stop(2);
sprintf(string," Y Into Range = %32f \r\n",Angle_Y); UART1_SendStr(string);
}
if(enter_range_X && enter_range_Y )break;
if(countTTT > 5000)break;
}
sprintf(string," Run Time is = %32ld \r\n",countTTT); UART1_SendStr(string);
}
//----------------------------------------------------功能修改~ 2010/04/17
void GoHome(){
long int cou = (long int)motor_T_AD * 1150; // 100Hz delay
while(cou > 0){ cou--; }
DoTheAd(AD_AVGtime,1);
double Ang_Buf_X, Ang_Buf_Y;
double MoveTime_X, MoveTime_Y; MoveTime_X = move1deg_T; MoveTime_Y = move1deg_T;
long int couX = -1, couY=-1;
bool closeX,closeY;
for(int hh=0;hh<RunTime;hh++){
closeX = false; closeY = false;
Ang_Buf_X = Angle_X;
Ang_Buf_Y = Angle_Y;
if( ( (abs(Angle_X)) < 0.1) && ( (abs(Angle_Y)) < 0.1) )break;
if(( (abs(Angle_X)) >= 0.1) ){
(Angle_X < 0) ? MotoControl_Run(1,CW) : MotoControl_Run(1,CCW);
couX = (long int)abs(MoveTime_X *1150);
sprintf(string," couX is %05ld \r\n",couX); UART1_SendStr(string);
}
if(( (abs(Angle_Y)) >= 0.1) ){
(Angle_Y < 0) ? MotoControl_Run(2,CW) : MotoControl_Run(2,CCW);
couY = (long int)abs(MoveTime_Y *1150);
sprintf(string," couY is %05ld \r\n",couY); UART1_SendStr(string);
}
while(1){
couX--; couY--;
if( (couX <0) && (!closeX)){closeX = true; MotoControl_Stop(1);}
if( (couY <0) && (!closeY)){closeY = true; MotoControl_Stop(2);}
if(closeX && closeY)break;
}
cou = (long int)motor_T_AD * 1150; while(cou > 0){ cou--; }
DoTheAd(AD_AVGtime,1);
if(Angle_X==Ang_Buf_X){MoveTime_X = move1deg_T;}
else{ MoveTime_X = (Angle_X * MoveTime_X) / (Angle_X - Ang_Buf_X); }
if(Angle_Y==Ang_Buf_Y){MoveTime_Y = move1deg_T;}
else{MoveTime_Y = (Angle_Y * MoveTime_Y) / (Angle_Y - Ang_Buf_Y);}
if(abs(MoveTime_X) > move1deg_T*5)MoveTime_X = move1deg_T; //-- 發散了
if(abs(MoveTime_Y) > move1deg_T*5)MoveTime_Y = move1deg_T;
sprintf(string," MoveTime_X is %09f MoveTime_Y is %09f \r\n",MoveTime_X,MoveTime_Y); UART1_SendStr(string);
sprintf(string," RunTime = %d \r\n",hh ); UART1_SendStr(string);
}
sprintf(string," End X is %05f End Y is %05f \r\n",Angle_X,Angle_Y); UART1_SendStr(string);
}
__interrupt void Timer_A (void)
{ // **** ***** **** 1Hz
if(WDT_flag)WDTCTL = WDT_ARST_1000; //--餵狗
if(startCount){ //---暫停讀秒 功能
divide++; if(divide>1){countTime++; divide=0;} //-.- 一秒
}
if(outPutcount && (divide==1)){ //每秒透過RS232輸出
FirstStart ?count_back = FirstStart_Sec - countTime : count_back = ReStart_Sec - countTime ;
sprintf(string,"%5ld s /per %2d hours \r",count_back,ReStart_Day);
// sprintf(string,"%5d heading /per %2d hours \r",TDCM3_Heading,TDCM3_status);
UART1_SendStr(string);
}
if( (countTime >= FirstStart_Sec ) && FirstStart){
outPutcount = false; //---做完第一次後就不用再輸出啦
FirstStart = false;
countTime = 0;
Setting_mode = false;
Leveling = true;
_BIC_SR_IRQ(LPM0_bits);
}
if( (countTime >= ReStart_Sec) &&(!FirstStart)){
Leveling = true;
countTime = 0;
_BIC_SR_IRQ(LPM0_bits);
}
}
//This function is UART0 Receive ISR. This functions is called whenever UART0 receives any data
void vsend(void *pvparam)
{
while(1)
{
UART1_SendStr(sen_dat);
UART1_SendByte(0x00);
UART1_SendByte(0xFF);
vTaskDelay(1000);
}
}
/*
init USART1
*/
void Hw_Usart_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
#if 0
USART_InitTypeDef USART_InitStructure;
USART_ClockInitTypeDef USART_ClockInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
//RCC
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOD, ENABLE);
//GPIO
/* Configure USART1 Rx (PA.10) as input floating */
GPIO_InitStructure.GPIO_Pin = UART1_GPIO_RX;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(UART1_GPIO, &GPIO_InitStructure);
/* Configure USART1 Tx (PA.09) as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = UART1_GPIO_TX;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(UART1_GPIO, &GPIO_InitStructure);
//NVIC
/* Enable the USART1 Interrupt */
NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
//USART Config
USART_InitStructure.USART_BaudRate = 9600;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_ClockInitStructure.USART_Clock = USART_Clock_Disable;
USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low;
USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge;
USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable;
USART_Init(USART1, &USART_InitStructure);
USART_ClockInit(USART1, &USART_ClockInitStructure);
/* enable interrupt */
USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
USART_Cmd(USART1, ENABLE);
UART1_SendStr("Start System.....\r\n",sizeof("Start System.....\r\n"));
UART1_SendStr("Welcome >>>>>>>>>\r\n",sizeof("Welcome >>>>>>>>>\r\n"));
UART1_SendStr("Over <<<<<<<<<<<<\r\n",sizeof("Over <<<<<<<<<<<<\r\n"));
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOD, &GPIO_InitStructure);
#endif
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
//======================时钟初始化=======================================
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_GPIOB, ENABLE);
GPIO_PinRemapConfig(GPIO_Remap_USART2, ENABLE);
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
//======================IO初始化=======================================
//USART2_TX
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
GPIO_Init(GPIOD, &GPIO_InitStructure);
//USART2_RX
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
GPIO_Init(GPIOD, &GPIO_InitStructure);
//配置485发送和接收模式
// TODO 暂时先写B13 等之后组网测试时再修改
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_Init(GPIOB, &GPIO_InitStructure);
//======================串口初始化=======================================
USART_InitStructure.USART_BaudRate = 9600;
//设置校验模式
USART_InitStructure.USART_Parity = USART_Parity_No;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_HardwareFlowControl =
USART_HardwareFlowControl_None;
USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
USART_Init(USART2, &USART_InitStructure);
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
USART_Cmd(USART2, ENABLE);
//=====================中断初始化======================================
//设置NVIC优先级分组为Group2：0-3抢占式优先级，0-3的响应式优先级
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
while(1)
{
USART_SendData(USART2, 0xaa);
}
}
__interrupt void usart1_rx (void)
{
command[coMMand_N] = RXBUF1;
coMMand_N++;
if(command[coMMand_N-1]==0x1B){ // ESC
coMMand_Mode = 0; coMMand_N = 0;
return;
}
if (command[coMMand_N-1]==0x0D){ // Enter
switch(coMMand_Mode){
case 0:
if(command[0] == 'k'){
coMMand_Mode = 99;
// string[0] = '\r' ;string[1] = '\n' ;
sprintf(string,"please select the function \r\n"); UART1_SendStr(string);
sprintf(string," ------V3.10------\r\n"); UART1_SendStr(string);
sprintf(string,"1. 控制模式((手動=0 自動回歸原點=1 %04d \r\n",ControlMode); UART1_SendStr(string);
sprintf(string,"2. 角度AD平均次數 = %04d \r\n",AD_AVGtime); UART1_SendStr(string);
sprintf(string,"3. 馬達轉動後讀取AD之延遲時間 = %04d \r\n",motor_T_AD); UART1_SendStr(string);
sprintf(string,"4. 原點_X = %04d \r\n",Set_OriginX); UART1_SendStr(string);
sprintf(string,"5. 原點_Y= %04d \r\n",Set_OriginY); UART1_SendStr(string);
sprintf(string,"6. 移動1度所需時間(請依據馬達特性修改 = %04d \r\n",move1deg_T); UART1_SendStr(string);
sprintf(string,"7. pwm_頻率 = %06d \r\n",frequency_pwm); UART1_SendStr(string);
sprintf(string,"8. pwm_百分比 = %05d (0~100) \r\n",percent_pwm); UART1_SendStr(string);
sprintf(string,"9. 自動找尋原點次數 = %05d (0~5000) \r\n",RunTime); UART1_SendStr(string);
sprintf(string,"a. 手動控制 移動X軸時間 = %05d (0~65000) \r\n",Hand_X_Time);UART1_SendStr(string);
sprintf(string,"b. 手動控制 移動X軸方向 = %05d (0~65000) \r\n",Hand_X_Dir); UART1_SendStr(string);
sprintf(string,"w. 手動控制 移動Y軸時間 = %05d (0~65000) \r\n",Hand_Y_Time);UART1_SendStr(string);
sprintf(string,"d. 手動控制 移動Y軸方向 = %05d (0~65000) \r\n",Hand_Y_Dir); UART1_SendStr(string);
sprintf(string,"e. 第一次啟動時間 = %05d (0~65000) 秒\r\n",FirstStart_Sec);UART1_SendStr(string);
sprintf(string,"f. 每隔%02d小時重新啟動 \r\n",ReStart_Day); UART1_SendStr(string);
sprintf(string,"g. 倒數暫停 (0暫停 1繼續 ) \r\n");UART1_SendStr(string);
sprintf(string," --------------------\r\n"); UART1_SendStr(string);
}
break;
case 99:
coMMand_Mode = command[0];
switch(coMMand_Mode){
case '1':
sprintf(string," ControlMode = (預設為1"); UART1_SendStr(string);
break;
case '2':
sprintf(string," AD_AVGtime = "); UART1_SendStr(string);
break;
case '3':
sprintf(string," motor_T_AD = "); UART1_SendStr(string);
break;
case '4':
sprintf(string," Set_OriginX= "); UART1_SendStr(string);
break;
case '5':
sprintf(string," Set_OriginY = "); UART1_SendStr(string);
break;
case '6':
sprintf(string," move1deg_T = "); UART1_SendStr(string);
break;
case '7':
sprintf(string," frequency_pwm = "); UART1_SendStr(string);
break;
case '8':
sprintf(string," percent_pwm = "); UART1_SendStr(string);
break;
case '9':
sprintf(string," RunTime = "); UART1_SendStr(string);
break;
case 'a':
sprintf(string," Hand_X_Time = "); UART1_SendStr(string);
break;
case 'b':
sprintf(string," Hand_X_Dir = "); UART1_SendStr(string);
break;
case 'w':
sprintf(string," Hand_Y_Time = "); UART1_SendStr(string);
break;
case 'd':
sprintf(string," Hand_Y_Dir = "); UART1_SendStr(string);
break;
case 'e':
sprintf(string," FirstStart_Sec = "); UART1_SendStr(string);
break;
case 'f':
sprintf(string," ReStart_Day = "); UART1_SendStr(string);
break;
case 'g':
sprintf(string," 0 oR 1 = "); UART1_SendStr(string);
break;
}
break;
case '1':
ControlMode =GetInputVal(coMMand_N);
break;
case '2':
AD_AVGtime = GetInputVal(coMMand_N); write_flash_segA();
break;
case '3':
motor_T_AD = GetInputVal(coMMand_N); write_flash_segA();
break;
case '4':
Set_OriginX = GetInputVal(coMMand_N);
if(Set_OriginX !=0)Set_OriginX_flag = 1; _BIC_SR_IRQ(LPM0_bits);
break;
case '5':
Set_OriginY = GetInputVal(coMMand_N);
if(Set_OriginY !=0)Set_OriginY_flag = 1; _BIC_SR_IRQ(LPM0_bits);
break;
case '6':
move1deg_T = GetInputVal(coMMand_N); write_flash_segA();
break;
case '7':
frequency_pwm = GetInputVal(coMMand_N); write_flash_segA();
break;
case '8':
percent_pwm = GetInputVal(coMMand_N); write_flash_segA();
break;
case '9':
RunTime = GetInputVal(coMMand_N); write_flash_segA();
break;
case 'a':
Hand_X_Time = GetInputVal(coMMand_N);
break;
case 'b':
Hand_X_Dir = GetInputVal(coMMand_N);
break;
case 'w':
Hand_Y_Time = GetInputVal(coMMand_N);
break;
case 'd':
Hand_Y_Dir = GetInputVal(coMMand_N);
break;
case 'e':
FirstStart_Sec = GetInputVal(coMMand_N); write_flash_segA();
break;
case 'f':
ReStart_Day = GetInputVal(coMMand_N); write_flash_segA();
break;
case 'g':
startCount = GetInputVal(coMMand_N);
break;
}
coMMand_N = 0;
}
}
//-----------------------------------------------------------------------------
void DoTheAd(int avgtime,bool print){
unsigned long int outPutX = 0;
unsigned long int outPutY = 0;
for(int y=0;y<avgtime;y++)
{
ADC12CTL0 |= ADC12SC; // Start conversion
while ((ADC12IFG & BIT2)==0); //---
// _BIS_SR(LPM0_bits + GIE);
results[0] = ADC12MEM1; // Move results, IFG is cleared
results[1] = ADC12MEM2; // Move results, IFG is cleared
outPutX += results[0];
outPutY += results[1];
}
outPutX = outPutX /avgtime;
outPutY = outPutY /avgtime;
Angle_X = (20 * ( (double)outPutX / 4096)) - 10 - ((double)Set_OriginX*0.01);
Angle_Y = (20 * ( (double)outPutY / 4096)) - 10 - ((double)Set_OriginY*0.01);
if(print){ sprintf(string," the X angle = %2.6f Y = %2.6f ",Angle_X,Angle_Y); UART1_SendStr(string);
sprintf(string," the X AD = %05d Y = %05d \r\n",(int)outPutX,(int)outPutY); UART1_SendStr(string);
}
}
//------------------------------------------------------------------------------
void main(void)
{
WDTCTL = WDTPW+WDTHOLD; // Stop watchdog timer
FCTL2 = FWKEY + FSSEL0 + FN0; // MCLK/2 for Flash Timing Generator
read_flash_segA();
Rs232_Init(); // RS232 初始化 115200 P3
TDCM3_Init(); // 電子羅盤 初始化
P4DIR = 0xff; // Set P4. to output direction
P4OUT = 0;
WDT_flag = true;
TBCTL = TBSSEL_2 + MC_1; // SCLK, up-down mode PWM timeClock
AD_Init();
WDTCTL = WDT_ARST_1000; //--開狗
CloseMotorPower(); //----開電測試系統
// TimerA 啟動讀秒
TACCTL0 = CCIE; // CCR0 interrupt enabled
TACCR0 = 16384-1; // for 0.5 Hz
TACTL = TASSEL_1 + MC_1; // ACLK, contmode
// _BIS_SR(GIE);
Setting_mode = true;
long int cou_delay;
while(Setting_mode){ //---- setting mode
if(Set_OriginX_flag){
Set_OriginX_flag = 0;
OpenMotorPower();
cou_delay = (long int)motor_T_AD * 1150; // delay
while(cou_delay > 0){ cou_delay--;}
DoTheAd(AD_AVGtime,1);
CloseMotorPower();
Set_OriginX = (int)(Angle_X*100);
write_flash_segA();
}
if(Set_OriginY_flag){
Set_OriginY_flag = 0;
OpenMotorPower();
cou_delay = (long int)motor_T_AD * 1150; // delay
while(cou_delay > 0){ cou_delay--;}
DoTheAd(AD_AVGtime,1);
CloseMotorPower();
Set_OriginY = (int)(Angle_Y*100);
write_flash_segA();
}
_BIS_SR(LPM0_bits + GIE);
}
IE2 &= ~URXIE1 ; // Disable USART1 RX interrupt
ReStart_Sec += ((unsigned long int)ReStart_Day * 3600); //--
// ReStart_Sec += ((unsigned long int)ReStart_Day * 60); //-- 每隔分鐘數
PwmSet();
int testRun = 0;
Leveling = true;
while(1) //-------------- working mode
{
if(Leveling){
WDTCTL = WDTPW+WDTHOLD; // Stop watchdog timer
TACCTL0 &= ~CCIE; // close timer
Leveling = false;
outPutcount = false;
//TDCM3_Enable();
testRun++;
sprintf(string,"this is %02d time \r\n",testRun); UART1_SendStr(string);
OpenMotorPower(); //----開電
WDTCTL = WDTPW+WDTHOLD; // Stop watchdog timer
long int cou = (long int)motor_T_AD * 1150; // 100Hz delay
while(cou > 0){ cou--; }
GoToRange_V2();
GoHome();
CloseMotorPower(); //----關電
outPutcount = true;
TACCTL0 |= CCIE; // open timer
}
_BIS_SR(LPM0_bits + GIE); // Enter LPM0, Enable interrupts
}
}