Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- Dynamixel AX12(1);
- int L,R,CR=2800,CL=2800;
- void setup(){
- InitAX12();
- InitIR();
- }
- void loop(){
- L=ADC(1);R=ADC(4);
- if(L>CL&&R>CR){
- fd(800);
- }
- else if(L<CL&&R>CR){
- sl(800);
- }
- else if(L>CL&&R<CR){
- sr(800);
- }
- else if(L<CL&&R<CR){
- fd(800);
- }
- }
- void InitAX12(){
- // Dynamixel 2.0 Protocol -> 0: 9600, 1: 57600, 2: 115200, 3: 1Mbps
- AX12.begin(3);
- // Disable CCW Angle Limit(L) to use wheel mode
- AX12.writeWord(1,8,0);
- AX12.writeWord(2,8,0);
- AX12.writeWord(3,8,0);
- AX12.writeWord(4,8,0);
- AX12.jointMode(5);
- }
- void fd(int speed){
- AX12.writeWord(1,32,speed);
- AX12.writeWord(2,32,speed);
- AX12.writeWord(3,32,speed|0x400);
- AX12.writeWord(4,32,speed|0x400);
- }
- void bk(int speed){
- AX12.writeWord(1,32,speed|0x400);
- AX12.writeWord(2,32,speed|0x400);
- AX12.writeWord(3,32,speed);
- AX12.writeWord(4,32,speed);
- }
- void sl(int speed){
- AX12.writeWord(1,32,speed);
- AX12.writeWord(2,32,speed);
- AX12.writeWord(3,32,speed);
- AX12.writeWord(4,32,speed);
- }
- void sr(int speed){
- AX12.writeWord(1,32,speed|0x400);
- AX12.writeWord(2,32,speed|0x400);
- AX12.writeWord(3,32,speed|0x400);
- AX12.writeWord(4,32,speed|0x400);
- }
- int ADC(unsigned char CH){
- int A;
- if(CH==1){
- digitalWrite(7,1);delayMicroseconds(15);
- A=analogRead(2);
- digitalWrite(7,0);
- return A;
- }
- else{
- digitalWrite(13,1);delayMicroseconds(15);
- A=analogRead(1);
- digitalWrite(13,0);
- return A;
- }
- }
- void InitIR(){
- pinMode(2, INPUT_ANALOG); // set pin 0 as analog input
- pinMode(1, INPUT_ANALOG); // set pin 0 as analog input
- pinMode(6,OUTPUT);
- pinMode(7,OUTPUT);
- pinMode(12,OUTPUT);
- pinMode(13,OUTPUT);
- digitalWrite(12,0);
- digitalWrite(13,0);
- digitalWrite(6,0);
- digitalWrite(7,0);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement