Pablo Paniagua / AX_12W

Program made to control Dynamixel servos type AX-12W. Works perfectly in the lpc1768

Dependents:   UNIBOT

DynamixelAX12.cpp@2:342f5327a1e0, 2019-03-01 (annotated)

Committer:
papaco
Date:
Fri Mar 01 18:30:30 2019 +0000
Revision:
2:342f5327a1e0
Parent:
0:e377b44007fb 
Updated on 3/1/19

Who changed what in which revision?

UserRevisionLine numberNew contents of line
papaco 0:e377b44007fb 1 #include "DynamixelAX12.h"
papaco 0:e377b44007fb 2 #include "mbed.h"
papaco 0:e377b44007fb 3
papaco 2:342f5327a1e0 4 Dynamixel::Dynamixel(void):serialPort(p13,p14),directionPin(p12,0){
papaco 0:e377b44007fb 5 serialPort.baud(1000000);
papaco 0:e377b44007fb 6 position=0;
papaco 0:e377b44007fb 7 velocity=0;
papaco 0:e377b44007fb 8 led=false;
papaco 0:e377b44007fb 9 serialDirection=false;
papaco 0:e377b44007fb 10 mode=false;
papaco 0:e377b44007fb 11 ID=0xFE;
papaco 0:e377b44007fb 12 }//Builder0
papaco 0:e377b44007fb 13
papaco 2:342f5327a1e0 14 Dynamixel::Dynamixel(unsigned int NewID):serialPort(p13,p14),directionPin(p12,0){
papaco 0:e377b44007fb 15 serialPort.baud(1000000);
papaco 0:e377b44007fb 16 position=0;
papaco 0:e377b44007fb 17 velocity=0;
papaco 0:e377b44007fb 18 led=false;
papaco 0:e377b44007fb 19 serialDirection=false;
papaco 0:e377b44007fb 20 mode=false;
papaco 0:e377b44007fb 21 ID=(unsigned char)(NewID & 0XFF);
papaco 0:e377b44007fb 22 }//Builder1
papaco 0:e377b44007fb 23
papaco 0:e377b44007fb 24 Dynamixel::Dynamixel(unsigned int NewID,PinName Newtx,PinName Newrx,PinName newdirectionPin,bool NewMode):serialPort(Newtx,Newrx),directionPin(newdirectionPin,1){
papaco 0:e377b44007fb 25 serialPort.baud(1000000);
papaco 0:e377b44007fb 26 position=0;
papaco 0:e377b44007fb 27 velocity=0;
papaco 0:e377b44007fb 28 led=false;
papaco 0:e377b44007fb 29 serialDirection=false;
papaco 0:e377b44007fb 30 ID=NewID;
papaco 0:e377b44007fb 31 mode=NewMode;
papaco 0:e377b44007fb 32 ID=(unsigned char)(NewID & 0XFF);
papaco 0:e377b44007fb 33 }//Builder2
papaco 0:e377b44007fb 34
papaco 0:e377b44007fb 35 void Dynamixel::SetLED(bool ledState){
papaco 0:e377b44007fb 36 char TxBuf[8];
papaco 0:e377b44007fb 37 TxBuf[0]=0xFF; //Start of incoming packet
papaco 0:e377b44007fb 38 TxBuf[1]=0xFF; //Start of incoming packet
papaco 0:e377b44007fb 39 TxBuf[2]=ID; //ID
papaco 0:e377b44007fb 40 TxBuf[3]=0x04; //Length = number of parameters + 2
papaco 0:e377b44007fb 41 TxBuf[4]=iWriteData; //Instruction
papaco 0:e377b44007fb 42 TxBuf[5]=cLED; //Parameter 1
papaco 0:e377b44007fb 43 if(ledState)
papaco 0:e377b44007fb 44 TxBuf[6]=0x01; //Parameter 2
papaco 0:e377b44007fb 45 else
papaco 0:e377b44007fb 46 TxBuf[6]=0x00; //Parameter 2
papaco 0:e377b44007fb 47 TxBuf[7]= ~(ID+TxBuf[3]+iWriteData+cLED+TxBuf[6]);
papaco 0:e377b44007fb 48 directionPin.write(1);
papaco 0:e377b44007fb 49 for(int i=0;i<8;i++){
papaco 0:e377b44007fb 50 serialPort.putc(TxBuf[i]);
papaco 0:e377b44007fb 51 }//for
papaco 0:e377b44007fb 52 wait_ms(10);
papaco 0:e377b44007fb 53 directionPin.write(0);
papaco 0:e377b44007fb 54 }//SetLED
papaco 0:e377b44007fb 55
papaco 0:e377b44007fb 56 void Dynamixel::SetMode(bool Mode){
papaco 0:e377b44007fb 57 char TxBuf[11];
papaco 0:e377b44007fb 58 TxBuf[0]=0xFF; //Start of incoming packet0
papaco 0:e377b44007fb 59 TxBuf[1]=0xFF; //Start of incoming packet1
papaco 0:e377b44007fb 60 TxBuf[2]=ID; //ID
papaco 0:e377b44007fb 61 TxBuf[3]=0x07; //Length = number of parameters + 2
papaco 0:e377b44007fb 62 TxBuf[4]=iWriteData; //Instruction
papaco 0:e377b44007fb 63 TxBuf[5]=cCWAngleLimitL; //Parameter 1
papaco 0:e377b44007fb 64 TxBuf[6]=0x00; //Parameter 2
papaco 0:e377b44007fb 65 TxBuf[7]=0x00; //Parameter 3
papaco 0:e377b44007fb 66 if(Mode){
papaco 0:e377b44007fb 67 TxBuf[8]=0x00; //Parameter 4
papaco 0:e377b44007fb 68 TxBuf[9]=0x00; //Parameter 5
papaco 0:e377b44007fb 69 }else{
papaco 0:e377b44007fb 70 TxBuf[8]=0xFF; //Parameter 4
papaco 0:e377b44007fb 71 TxBuf[9]=0x03; //Parameter 5
papaco 0:e377b44007fb 72 }//else
papaco 0:e377b44007fb 73 TxBuf[10]= ~(ID+TxBuf[3]+iWriteData+cCWAngleLimitL+TxBuf[6]+TxBuf[7]+TxBuf[8]+TxBuf[9]);
papaco 0:e377b44007fb 74 directionPin.write(1);
papaco 0:e377b44007fb 75 for(int i=0;i<11;i++){
papaco 0:e377b44007fb 76 serialPort.putc(TxBuf[i]);
papaco 0:e377b44007fb 77 }//for
papaco 0:e377b44007fb 78 wait_ms(10);
papaco 0:e377b44007fb 79 directionPin.write(0);
papaco 0:e377b44007fb 80 }//SetMode
papaco 0:e377b44007fb 81
papaco 0:e377b44007fb 82 void Dynamixel::SetPosition(unsigned long Newposition){
papaco 0:e377b44007fb 83 char TxBuf[9];
papaco 0:e377b44007fb 84 TxBuf[0]=0xFF; //Start of incoming packet0
papaco 0:e377b44007fb 85 TxBuf[1]=0xFF; //Start of incoming packet1
papaco 0:e377b44007fb 86 TxBuf[2]=ID; //ID
papaco 0:e377b44007fb 87 TxBuf[3]=0x05; //Length = number of parameters + 2
papaco 0:e377b44007fb 88 TxBuf[4]=iWriteData; //Instruction
papaco 0:e377b44007fb 89 TxBuf[5]=cGoalPositionL; //Parameter 1
papaco 0:e377b44007fb 90 TxBuf[6]=(unsigned char)(Newposition & 0XFF);
papaco 0:e377b44007fb 91 TxBuf[7]=(unsigned char)((Newposition >> 8) & 0XFF);
papaco 0:e377b44007fb 92 TxBuf[8]= ~(ID+TxBuf[3]+iWriteData+cGoalPositionL+TxBuf[6]+TxBuf[7]);
papaco 0:e377b44007fb 93 directionPin.write(1);
papaco 0:e377b44007fb 94 for(int i=0;i<9;i++){
papaco 0:e377b44007fb 95 serialPort.putc(TxBuf[i]);
papaco 0:e377b44007fb 96 }//for
papaco 0:e377b44007fb 97 wait_ms(10);
papaco 0:e377b44007fb 98 directionPin.write(0);
papaco 0:e377b44007fb 99 }//SetPosition
papaco 0:e377b44007fb 100
papaco 0:e377b44007fb 101 void Dynamixel::SetVelocity(unsigned long Newvelocity){
papaco 0:e377b44007fb 102 char TxBuf[9];
papaco 0:e377b44007fb 103 TxBuf[0]=0xFF; //Start of incoming packet0
papaco 0:e377b44007fb 104 TxBuf[1]=0xFF; //Start of incoming packet1
papaco 0:e377b44007fb 105 TxBuf[2]=ID; //ID
papaco 0:e377b44007fb 106 TxBuf[3]=0x05; //Length = number of parameters + 2
papaco 0:e377b44007fb 107 TxBuf[4]=iWriteData; //Instruction
papaco 0:e377b44007fb 108 TxBuf[5]=cMovingSpeedL; //Parameter 1
papaco 0:e377b44007fb 109 TxBuf[6]=(unsigned char)(Newvelocity & 0XFF);
papaco 0:e377b44007fb 110 TxBuf[7]=(unsigned char)((Newvelocity >> 8) & 0XFF);
papaco 0:e377b44007fb 111 TxBuf[8]= ~(ID+TxBuf[3]+iWriteData+TxBuf[5]+TxBuf[6]+TxBuf[7]);
papaco 0:e377b44007fb 112 directionPin.write(1);
papaco 0:e377b44007fb 113 for(int i=0;i<9;i++){
papaco 0:e377b44007fb 114 serialPort.putc(TxBuf[i]);
papaco 0:e377b44007fb 115 }//for
papaco 0:e377b44007fb 116 wait_ms(10);
papaco 0:e377b44007fb 117 directionPin.write(0);
papaco 0:e377b44007fb 118 }//SetVelocity
papaco 0:e377b44007fb 119
papaco 0:e377b44007fb 120 void Dynamixel::SetDirection(bool Mode){
papaco 0:e377b44007fb 121 char TxBuf[9];
papaco 0:e377b44007fb 122 TxBuf[0]=0xFF; //Start of incoming packet0
papaco 0:e377b44007fb 123 TxBuf[1]=0xFF; //Start of incoming packet1
papaco 0:e377b44007fb 124 TxBuf[2]=ID; //ID
papaco 0:e377b44007fb 125 TxBuf[3]=0x05; //Length = number of parameters + 2
papaco 0:e377b44007fb 126 TxBuf[4]=iWriteData; //Instruction
papaco 0:e377b44007fb 127 TxBuf[5]=cMovingSpeedL; //Parameter 1
papaco 0:e377b44007fb 128 TxBuf[6]=0xFF; //Parameter 2
papaco 0:e377b44007fb 129 if(Mode)
papaco 0:e377b44007fb 130 TxBuf[7]=0x03; //Parameter 3
papaco 0:e377b44007fb 131 else
papaco 0:e377b44007fb 132 TxBuf[7]=0x07; //Parameter 3
papaco 0:e377b44007fb 133 TxBuf[8]= ~(ID+TxBuf[3]+iWriteData+cCWAngleLimitL+TxBuf[6]+TxBuf[7]);
papaco 0:e377b44007fb 134 directionPin.write(1);
papaco 0:e377b44007fb 135 for(int i=0;i<9;i++){
papaco 0:e377b44007fb 136 serialPort.putc(TxBuf[i]);
papaco 0:e377b44007fb 137 }//for
papaco 0:e377b44007fb 138 wait_ms(10);
papaco 0:e377b44007fb 139 directionPin.write(0);
papaco 0:e377b44007fb 140 }//SetDirection
papaco 0:e377b44007fb 141
papaco 0:e377b44007fb 142 void Dynamixel::move(){
papaco 0:e377b44007fb 143 char TxBuf[16];
papaco 0:e377b44007fb 144 TxBuf[0]=0xFF; //Start of incoming packet0
papaco 0:e377b44007fb 145 TxBuf[1]=0xFF; //Start of incoming packet1
papaco 0:e377b44007fb 146 TxBuf[2]=0xFE; //ID
papaco 0:e377b44007fb 147 TxBuf[3]=0x02; //Length = number of parameters + 2
papaco 0:e377b44007fb 148 TxBuf[4]=iAction; //Instruction
papaco 0:e377b44007fb 149 TxBuf[5]=0xFA;
papaco 0:e377b44007fb 150 directionPin.write(1);
papaco 0:e377b44007fb 151 for(int i=0;i<6;i++){
papaco 0:e377b44007fb 152 serialPort.putc(TxBuf[i]);
papaco 0:e377b44007fb 153 }//for
papaco 0:e377b44007fb 154 wait_ms(10);
papaco 0:e377b44007fb 155 directionPin.write(0);
papaco 0:e377b44007fb 156 }//move