Dyanamixel
Dependents: YOZAKURA_ARM YOZAKURA_ARM_USB YOZAKURA_ARM_USB_Keyboard YOZAKURA_ARM_Keyboard0424 ... more
Diff: AX12.cpp
- Revision:
- 6:5ce46b6748fd
- Parent:
- 5:950c4f4f73c6
- Child:
- 9:0927a7b74806
- Child:
- 10:8ed335824731
diff -r 950c4f4f73c6 -r 5ce46b6748fd AX12.cpp --- a/AX12.cpp Tue Apr 21 16:08:24 2015 +0000 +++ b/AX12.cpp Wed Apr 22 18:06:00 2015 +0000 @@ -25,8 +25,6 @@ #include "mbed.h" //#include "SerialHalfDuplex.h" -Serial pc2(USBTX, USBRX); // tx, rx - AX12::AX12(PinName tx, PinName rx, int ID, int baudrate) : _ax12(tx,rx) { @@ -94,6 +92,10 @@ int goal = (0x3ff * abs(speed)); + if (AX12_DEBUG) { + printf("SetCRSpeed to 0x%x\n",goal); + } + // Set direction CW if we have a negative speed if (speed < 0) { goal |= (0x1 << 10); @@ -110,7 +112,6 @@ int AX12::SetCWLimit (int degrees) { - printf("SetCWLimit to %d\n",degrees); char data[2]; @@ -227,7 +228,6 @@ float AX12::GetPosition(void) { - pc2.printf("\nGetPosition(ID%d)",_ID); if (AX12_DEBUG) { printf("\nGetPosition(%d)",_ID); @@ -236,13 +236,9 @@ char data[2]; int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data); - pc2.printf("\n1"); short position = data[0] + (data[1] << 8); - pc2.printf("\n2"); float angle = (position * 300)/1024; - pc2.printf("\n3"); - pc2.printf("\nGetPosition(angle%f)\n",angle); return (angle); } @@ -353,11 +349,9 @@ } // Transmit the packet in one burst with no pausing - pc2.printf("read:transmit the packet"); for (int i = 0; i<8 ; i++) { _ax12.putc(TxBuf[i]); } - pc2.printf("read:transmit the packet : end"); // Wait for the bytes to be transmitted wait (0.00002); @@ -381,7 +375,7 @@ printf(" Header : 0x%x\n",Status[1]); printf(" ID : 0x%x\n",Status[2]); printf(" Length : 0x%x\n",Status[3]); - pc2.printf(" Error Code : 0x%x\n",Status[4]); + printf(" Error Code : 0x%x\n",Status[4]); for (int i=0; i < Status[3]-2 ; i++) { printf(" Data : 0x%x\n",Status[5+i]); @@ -467,7 +461,6 @@ } // Transmit the packet in one burst with no pausing - pc2.printf("transmit the packet\n"); for (int i = 0; i < (7 + bytes) ; i++) { _ax12.putc(TxBuf[i]); }