- void System_IO_Init()
- {
- pinMode(DIR_pin, OUTPUT);
- pinMode(DIR2_pin, OUTPUT);
- pinMode(DIR3_pin, OUTPUT);
- pinMode(PUL_pin, OUTPUT);
- pinMode(PUL2_pin, OUTPUT);
- pinMode(PUL3_pin, OUTPUT);
- pinMode(ENA_pin, OUTPUT);
- pinMode(ENA2_pin, OUTPUT);
- pinMode(ENA3_pin, OUTPUT);
- pinMode(STOPPER_x_pin, INPUT_PULLUP);
- pinMode(STOPPER_x_2_pin, INPUT_PULLUP);
- pinMode(STOPPER_y_pin, INPUT_PULLUP);
- pinMode(STOPPER_y_2_pin, INPUT_PULLUP);
- pinMode(STOPPER_z_pin, INPUT_PULLUP);
-
- digitalWrite( DIR_pin, HIGH); //HIGH 左 x轴
- digitalWrite( ENA_pin, LOW);
- digitalWrite( DIR2_pin, HIGH); //HIGH 左
- digitalWrite( ENA2_pin, LOW);
- digitalWrite( DIR3_pin, HIGH); //HIGH 左
- digitalWrite( ENA3_pin, LOW);
- }
- void Timer_Init()
- {
- cli();////关闭全局中断
- //Timer 1
- //设置定时器1为1kHz
- TCCR1A = 0;//将整个TCCR1A寄存器设置为0
- TCCR1B = 0;//将整个TCCR1B寄存器设置为0
- TCNT1 = 0;//将计数器值初始化为0
- //设置计数器为10kHZ,即1ms
- OCR1A = 19;// = (16*10^6)/(1000*8) - 1 (must be <65536)
- TCCR1B |= (1 << WGM12);//打开CTC模式
- TCCR1B |= (1 << CS11);//设置CS11位为1(8倍预分频)
- TIMSK1 |= (1 << OCIE1A);
- //Timer 3
- //设置定时器3为1kHz
- TCCR3A = 0;//将整个TCCR1A寄存器设置为0
- TCCR3B = 0;//将整个TCCR1B寄存器设置为0
- TCNT3 = 0;//将计数器值初始化为0
- //设置计数器为10kHZ,即1ms
- OCR3A = 19;// = (16*10^6)/(1000*8) - 1 (must be <65536)
- TCCR3B |= (1 << WGM32);//打开CTC模式
- TCCR3B |= (1 << CS31);//设置CS11位为1(8倍预分频)
- TIMSK3 |= (1 << OCIE3A);
- sei();//打开全局中断
- }
- void return_weizhi()
- {
- int test=1;
- digitalWrite( DIR_pin, LOW); //HIGH 左 LOW 右
- digitalWrite( DIR2_pin, LOW);
- while(test)
- {
- while(((PINH & (B00100000)) == 32)||((PING & (B00100000)) == 32))
- {
- if((PINH & (B00100000)) == 32)
- y_flag=1;
- else
- y_flag=0;
- if((PING & (B00100000)) == 32)
- x_flag=1;
- else
- x_flag=0;
- // Serial.println("555");
- if(((PINH & (B00100000)) == 0)&&((PING & (B00100000)) == 0))
- {
- delay(100);
- // Serial.println("444");
- if(((PINH & (B00100000)) == 0)&&((PING & (B00100000)) == 0))
- {
- delay(100);
- if(((PINH & (B00100000)) == 0)&&((PING & (B00100000)) == 0))
- {
- x_flag=0;
- y_flag=0;
- x_pos=0;
- y_pos=0;
- Serial.println("{X000000-Y000000}");
- test=0;
- break;
- }
- }
- }
- }
- test=0;
- }
- }
- void setup() { // put your setup code here, to run once:
- System_IO_Init();
- Timer_Init();
- Serial.begin(115200);
- return_weizhi(); //系统返回原点
- }
- void SET_MOTOR_rotation(long num,long speed_motor) //设置固定转多少圈,用于测试
- {
- for(i=0;i<num;i++)
- {
- digitalWrite( PUL_pin, HIGH);
- delayMicroseconds(speed_motor);
- digitalWrite( PUL_pin, LOW);
- delayMicroseconds(speed_motor);
- }
- }
- ISR(TIMER1_COMPA_vect) //定时器1,用于触发x,y轴电机转动
- {
- //产生频率为1Hz / 2 = 0.5Hz的脉冲波(全波切换为两个周期,然后切换为低)
- if(x_motor_en==1&&x_position<=11800)
- {
- if(time_pul1<x_position)
- {
- if(toggle1<15)
- digitalWrite(PUL_pin,HIGH);
- if(toggle1>=15)
- digitalWrite(PUL_pin,LOW);
-
- toggle1 += 1;
- if((motor1_time_set>motor1_time)&&time_pul1%16==1) //加速
- motor1_time_set-=1;
- if(toggle1 > 15*2)
- {
- toggle1 = 0;
- if(x_fangxiang)
- x_pos++;
- else
- x_pos--;
- time_pul1++;
- }
- }
- else
- {
- pinMode(PUL_pin, INPUT_PULLUP);
-
- time_pul1=0;
- x_motor_en=0;
- sdsf++;
- reback_weizhi_flag=1;
- }
- }
- if(x_flag==1&&(digitalRead(STOPPER_y_pin) == 1))
- {
-
- if(toggle1<30)
- digitalWrite(PUL_pin,HIGH);
- if(toggle1>=30)
- digitalWrite(PUL_pin,LOW);
- toggle1 += 1;
- if(toggle1 > 30*2)
- {
- toggle1 = 0;
- }
- }
-
- if(y_motor_en==1&&y_position<=11000)
- {
- if(time_pul2<y_position) //5.4cm 54mm
- {
- if(toggle2<15)
- digitalWrite(PUL2_pin,HIGH);
- if(toggle2>=15)
- digitalWrite(PUL2_pin,LOW);
- toggle2 += 1;
- if((motor2_time_set>motor2_time)&&time_pul2%16==1) //加速
- motor2_time_set-=1;
-
- if(toggle2 > 15*2)
- {
- toggle2 = 0;
- if(y_fangxiang)
- y_pos++;
- else
- y_pos--;
- time_pul2++;
- }
- }
- else
- {
- pinMode(PUL2_pin, INPUT_PULLUP);
-
- time_pul2=0;
- y_motor_en=0;
- sdsf++;
- reback_weizhi_flag=1;
- }
- }
- if(y_flag==1&&(digitalRead(STOPPER_x_pin) == 1))
- {
- if(toggle2<30)
- digitalWrite(PUL2_pin,HIGH);
- if(toggle2>=30)
- digitalWrite(PUL2_pin,LOW);
- toggle2 += 1;
- if(toggle2 > 30*2)
- {
- toggle2 = 0;
- }
- }
- }
- ISR(TIMER3_COMPA_vect) //定时器3,用于触发z轴电机转动
- {
- // timer3中断1Hz切换引脚9
- //产生频率为1Hz / 2 = 0.5Hz的脉冲波(全波切换为两个周期,然后切换为低)
-
- if(z_motor_en==1&&z_position<=5000)
- {
- if(time_pul3<z_position) //5.4cm 54mm
- {
- // if(toggle3<15)
- // digitalWrite(PUL3_pin,HIGH);
- // if(toggle3>=15)
- // digitalWrite(PUL3_pin,LOW);
- // toggle3 += 1;
- //
- // if((motor2_time_set>motor2_time)&&time_pul3%16==1) //加速
- // motor2_time_set-=1;
- //
- // if(toggle3 > 15*2)
- // {
- // toggle3 = 0;
- // if(z_fangxiang)
- // z_pos++;
- // else
- // z_pos--;
- // time_pul3++;
- // }
- }
- else
- {
- // time_pul3=0;
- // z_motor_en=0;
- // sdsf++;
- // reback_weizhi_flag=1;
- }
- }
- }
- void goto_setposition(long x_now,long y_now)
- {
- /*
- int x_position_pass=0; //上一次的x轴位置坐标
- int y_position_pass=0; //上一次的y轴位置坐标
- int x_position_now=0; //当前的x轴位置坐标
- int y_position_now=0; //当前的y轴位置坐标
- int x_position=10; //x轴位置坐标
- int y_position=10; //z轴位置坐标
- */
-
- if(x_now!=0)
- {
- x_position_now=x_now;
- if(x_position_pass-x_position_now>0)
- {
- digitalWrite(DIR_pin,LOW); //设置方向
- x_fangxiang=0;
- x_position=x_position_pass-x_position_now; //计算步距
- return_x_position=x_position;
- x_position_pass=x_position_now;
- x_motor_en=1;
- }
- else
- {
- digitalWrite(DIR_pin,HIGH); //设置方向
- x_fangxiang=1;
- x_position=x_position_now-x_position_pass; //计算步距
- x_position_pass=x_position_now;
- x_motor_en=1;
- }
- }
- if(y_now!=0)
- {
- y_position_now=y_now;
- if(y_position_pass-y_position_now>0)
- {
- digitalWrite(DIR2_pin,LOW); //设置方向
- y_fangxiang=0;
- y_position=y_position_pass-y_position_now; //计算步距
- y_position_pass=y_position_now;
- y_motor_en=1;
- }
- else
- {
- digitalWrite(DIR2_pin,HIGH); //设置方向
- y_fangxiang=1;
- y_position=y_position_now-y_position_pass; //计算步距
- y_position_pass=y_position_now;
- y_motor_en=1;
- }
- }
- }
- void sys_auto_move()
- {
- if(sdsf==2)
- {
- if(!switch_flag)
- Location_label++;
- else
- Location_label--;
-
- if(Location_label>12)
- {
- Location_label=12;
- switch_flag=!switch_flag;
- }
- if(Location_label<1)
- {
- Location_label=1;
- //////////////////////////////////////////
- auto_manual_flag=0;
- delay(1000);
- delay(1000);
- pinMode(PUL_pin, OUTPUT);
- pinMode(PUL2_pin, OUTPUT);
- return_weizhi();
- x_position_now=0;
- y_position_now=0;
- x_position_pass=0;
- y_position_pass=0;
- x_pos=0;
- y_pos=0;
- Serial.print("{D0}\r\n");
- delay(1000);
- delay(1000);
- delay(1000);
- delay(1000);
- auto_manual_flag=1;
- auto_move_flag=1;
- sdsf=0;
- ///////////////////////////////////////////////
-
- switch_flag=!switch_flag;
- cishu_cishu++;
- }
- sdsf=0;
- auto_move_flag=1;
- delay(1000);
- delay(1000);
- delay(1000);
- delay(1000);
- delay(1000);
- }
- if(auto_move_flag==1)
- {
- pinMode(PUL_pin, OUTPUT);
- pinMode(PUL2_pin, OUTPUT);
- switch(Location_label)
- {
- case 1: actual_weizhi=1;goto_setposition(1500,hang1);auto_move_flag=0; break;
- case 2: actual_weizhi=2;goto_setposition(1500+1500,hang1); auto_move_flag=0;break;
- case 3: actual_weizhi=3;goto_setposition(1500+1500*2,hang1); auto_move_flag=0;break;
- case 4: actual_weizhi=4;goto_setposition(1500+1500*3,hang1); auto_move_flag=0;break;
-
- case 8: actual_weizhi=5;goto_setposition(1500,hang2);auto_move_flag=0; break;
- case 7: actual_weizhi=6;goto_setposition(1500+1500,hang2); auto_move_flag=0;break;
- case 6: actual_weizhi=7;goto_setposition(1500+1500*2,hang2); auto_move_flag=0;break;
- case 5: actual_weizhi=8;goto_setposition(1500+1500*3,hang2); auto_move_flag=0;break;
-
- case 9: actual_weizhi=13;goto_setposition(1500,hang3);auto_move_flag=0; break;
- case 10: actual_weizhi=14;goto_setposition(1500+1500,hang3); auto_move_flag=0;break;
- case 11: actual_weizhi=15;goto_setposition(1500+1500*2,hang3); auto_move_flag=0;break;
- case 12: actual_weizhi=16;goto_setposition(1500+1500*3,hang3); auto_move_flag=0;break;
- }
- }
- }
- void data_pro() //串口数据接受与处理
- {
- if(Serial.available()>0)
- {
- receive_buf[UART_RX_DATA_SIZE]=Serial.read();
- if(receive_buf[UART_RX_DATA_SIZE]=='}')
- {
- UAER_RX_FLAG=1;
- i_uart=0;
- }
- UART_RX_DATA_SIZE++;
-
- }
- else if(UAER_RX_FLAG)
- {
- UAER_RX_FLAG=0;
- #if UART_DEBUG
- Serial.println(receive_buf);
- #endif
- while(i_uart<UART_RX_DATA_SIZE)
- {
- if(receive_buf[i_uart]=='{') //{A0}
- {
- if(receive_buf[i_uart+1]=='A'&&receive_buf[i_uart+3]=='}') //收到上位机发送的HALLO指令
- {
- Serial.print("{B0}\r\n");
- i_uart+=3;
- }
- else if(receive_buf[i_uart+1]=='C'&&receive_buf[i_uart+3]=='}') //全部軸歸回原點
- {
- pinMode(PUL_pin, OUTPUT);
- pinMode(PUL2_pin, OUTPUT);
- return_weizhi();
- x_position_now=0;
- y_position_now=0;
- x_position_pass=0;
- y_position_pass=0;
- x_pos=0;
- y_pos=0;
- Serial.print("{D0}\r\n");
- i_uart+=3;
- }
- else if(receive_buf[i_uart+1]=='E'&&receive_buf[i_uart+13]=='}') //{EX000400-010}
- {
- pinMode(PUL_pin, OUTPUT);
- pinMode(PUL2_pin, OUTPUT);
- if(receive_buf[i_uart+2]=='X')
- {
- x_position_uart=(receive_buf[i_uart+3]-'0')*100000+(receive_buf[i_uart+4]-'0')*10000+(receive_buf[i_uart+5]-'0')*1000+(receive_buf[i_uart+6]-'0')*100+(receive_buf[i_uart+7]-'0')*10+(receive_buf[i_uart+8]-'0');
- #if UART_DEBUG
- Serial.println(x_position_uart);
- #endif
- goto_setposition(x_position_uart,0);
- Serial.print("{F0}\r\n");
- }
- else if(receive_buf[i_uart+2]=='Y')
- {
- y_position_uart=(receive_buf[i_uart+3]-'0')*100000+(receive_buf[i_uart+4]-'0')*10000+(receive_buf[i_uart+5]-'0')*1000+(receive_buf[i_uart+6]-'0')*100+(receive_buf[i_uart+7]-'0')*10+(receive_buf[i_uart+8]-'0');
- #if UART_DEBUG
- Serial.println(y_position_uart);
- #endif
- goto_setposition(0,y_position_uart);
- Serial.print("{F0}\r\n");
- }
- i_uart+=13;
- }
- else if(receive_buf[i_uart+1]=='L'&&receive_buf[i_uart+4]=='}') //{L01}
- {
- int num=(receive_buf[i_uart+2]-'0')*10+(receive_buf[i_uart+3]-'0');
- pinMode(PUL_pin, OUTPUT);
- pinMode(PUL2_pin, OUTPUT);
- switch(num)
- {
- case 1: actual_weizhi=1;goto_setposition(1500,hang1);auto_move_flag=0; break;
- case 2: actual_weizhi=2;goto_setposition(1500+1500,hang1); auto_move_flag=0;break;
- case 3: actual_weizhi=3;goto_setposition(1500+1500*2,hang1); auto_move_flag=0;break;
- case 4: actual_weizhi=4;goto_setposition(1500+1500*3,hang1); auto_move_flag=0;break;
-
- case 5: actual_weizhi=5;goto_setposition(1500,hang2);auto_move_flag=0; break;
- case 6: actual_weizhi=6;goto_setposition(1500+1500,hang2); auto_move_flag=0;break;
- case 7: actual_weizhi=7;goto_setposition(1500+1500*2,hang2); auto_move_flag=0;break;
- case 8: actual_weizhi=8;goto_setposition(1500+1500*3,hang2); auto_move_flag=0;break;
-
- case 9: actual_weizhi=13;goto_setposition(1500,hang3);auto_move_flag=0; break;
- case 10: actual_weizhi=14;goto_setposition(1500+1500,hang3); auto_move_flag=0;break;
- case 11: actual_weizhi=15;goto_setposition(1500+1500*2,hang3); auto_move_flag=0;break;
- case 12: actual_weizhi=16;goto_setposition(1500+1500*3,hang3); auto_move_flag=0;break;
-
- }
- i_uart+=4;
- }
- else if(receive_buf[i_uart+1]=='M'&&receive_buf[i_uart+3]=='}') //{M0}
- {
- if(receive_buf[i_uart+2]=='1')
- {
- auto_manual_flag=1;
- auto_move_flag=1;
- sdsf=0;
- Serial.println("auto_mode");
- }
- else
- {
- auto_manual_flag=0;
- }
- i_uart+=3;
- }
- else if(receive_buf[i_uart+1]=='F'&&receive_buf[i_uart+8]=='}') //{F0_2500} 设定相机焦距
- {
- //步进值 16位
- //左右旋转标志位 Direction_flag
- direction_flag=receive_buf[i_uart+2]-0x30;
- Serial.println(direction_flag);
- focal_step_value=(receive_buf[i_uart+4]-0x30)*1000+(receive_buf[i_uart+5]-0x30)*100+(receive_buf[i_uart+6]-0x30)*10+(receive_buf[i_uart+7]-0x30);
- focal_step_value=focal_step_value/10;
- Serial.println(focal_step_value);
- z_motor_en=1;
-
- if(direction_flag==0)
- {
- z_position=focal_step_value;
- digitalWrite(DIR3_pin,HIGH);
-
- }
- else if(direction_flag==1)
- {
- z_position=focal_step_value;
- digitalWrite(DIR3_pin,LOW);
-
- }
- i_uart+=8;
- }
-
-
- }
- i_uart++;
- }
- for(i=0;i<UART_RX_DATA_SIZE;i++)
- receive_buf[i]=' ';
- UART_RX_DATA_SIZE=0;
- }
- }
- void loop()
- {
- data_pro();
- // if(digitalRead(STOPPER_x_pin)==1)
- // Serial.println("111");
- // else
- // Serial.println("222");
-
- if(auto_manual_flag)
- {
- sys_auto_move();
- }
- if(reback_weizhi_flag)
- {
- sprintf(weizhi_data,"{X%06ld-Y%06ld-L%02d}",x_pos,y_pos,actual_weizhi);
- Serial.println(weizhi_data);
- reback_weizhi_flag=0;
- }
- delay(100);
- }
复制代码
这是所有程序了 |