Revision 1:9dd052490f0a, committed 2019-02-04

Comitter:

sink

Date:

Mon Feb 04 04:46:37 2019 +0000

Parent:

0:03e99bd9339e

Commit message:

ok

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw.cpp	Mon Feb 04 04:46:37 2019 +0000
@@ -0,0 +1,251 @@
+#include "RoboClaw.h"
+#include <stdarg.h>
+
+#define MAXTRY 1
+#define SetDWORDval(arg) (uint8_t)(arg>>24),(uint8_t)(arg>>16),(uint8_t)(arg>>8),(uint8_t)arg
+#define SetWORDval(arg) (uint8_t)(arg>>8),(uint8_t)arg
+
+RoboClaw::RoboClaw(int baudrate, PinName rx, PinName tx) : _roboclaw(rx, tx){
+    _roboclaw.baud(baudrate);
+}
+
+void RoboClaw::crc_clear(){
+    crc = 0;
+}
+
+void RoboClaw::crc_update (uint8_t data){
+    int i;
+    crc = crc ^ ((uint16_t)data << 8);
+    for (i=0; i<8; i++) {
+        if (crc & 0x8000)
+            crc = (crc << 1) ^ 0x1021;
+        else
+            crc <<= 1;
+    }
+}
+
+uint16_t RoboClaw::crc_get(){
+    return crc;
+}
+
+void RoboClaw::write_n(uint8_t cnt, ... ){
+    //uint8_t retry = MAXTRY;
+    //do {
+        crc_clear();
+        va_list marker;
+        va_start( marker, cnt );
+        for(uint8_t index=0; index<cnt; index++) {
+            uint8_t data = va_arg(marker, unsigned int);
+            crc_update(data);
+            _roboclaw.putc(data);
+        }
+        va_end( marker );
+        uint16_t crc = crc_get();
+        _roboclaw.putc(crc>>8);
+        _roboclaw.putc(crc);
+    //} while(_roboclaw.getc() != 0xFF);
+}
+
+void RoboClaw::write_(uint8_t address, uint8_t command, uint8_t data, bool reading, bool crcon){
+    _roboclaw.putc(address);
+    _roboclaw.putc(command);
+
+    if(reading == false) {
+        if(crcon == true) {
+            uint8_t packet[2] = {address, command};
+            uint16_t checksum = crc16(packet,2);
+            _roboclaw.putc(checksum>>8);
+            _roboclaw.putc(checksum);
+        } else {
+            uint8_t packet[3] = {address, command, data};
+            uint16_t checksum = crc16(packet,3);
+            _roboclaw.putc(data);
+            _roboclaw.putc(checksum>>8);
+            _roboclaw.putc(checksum);
+        }
+    }
+}
+
+uint16_t RoboClaw::crc16(uint8_t *packet, int nBytes){
+    uint16_t crc_;
+    for (int byte = 0; byte < nBytes; byte++) {
+        crc_ = crc_ ^ ((uint16_t)packet[byte] << 8);
+        for (uint8_t bit = 0; bit < 8; bit++) {
+            if (crc_ & 0x8000) {
+                crc_ = (crc_ << 1) ^ 0x1021;
+            } else {
+                crc_ = crc_ << 1;
+            }
+        }
+    }
+    return crc_;
+}
+
+uint8_t RoboClaw::read_(void){
+    return(_roboclaw.getc());
+}
+
+void RoboClaw::ForwardM1(uint8_t address, int speed){
+    write_(address,M1FORWARD,speed,false,false);
+}
+
+void RoboClaw::BackwardM1(uint8_t address, int speed){
+    write_(address,M1BACKWARD,speed,false,false);
+}
+
+void RoboClaw::ForwardM2(uint8_t address, int speed){
+    write_(address,M2FORWARD,speed,false,false);
+}
+
+void RoboClaw::BackwardM2(uint8_t address, int speed){
+    write_(address,M2BACKWARD,speed,false,false);
+}
+
+void RoboClaw::Forward(uint8_t address, int speed){
+    write_(address,MIXEDFORWARD,speed,false,false);
+}
+
+void RoboClaw::Backward(uint8_t address, int speed){
+    write_(address,MIXEDBACKWARD,speed,false,false);
+}
+
+void RoboClaw::ReadFirm(uint8_t address){
+    write_(address,GETVERSION,0x00,true,false);
+}
+
+int32_t RoboClaw::ReadEncM1(uint8_t address){
+    int32_t enc1;
+    uint16_t read_byte[7];
+    write_n(2,address,GETM1ENC);
+
+    read_byte[0] = (uint16_t)_roboclaw.getc();
+    read_byte[1] = (uint16_t)_roboclaw.getc();
+    read_byte[2] = (uint16_t)_roboclaw.getc();
+    read_byte[3] = (uint16_t)_roboclaw.getc();
+    read_byte[4] = (uint16_t)_roboclaw.getc();
+    read_byte[5] = (uint16_t)_roboclaw.getc();
+    read_byte[6] = (uint16_t)_roboclaw.getc();
+
+    enc1 = read_byte[1]<<24;
+    enc1 |= read_byte[2]<<16;
+    enc1 |= read_byte[3]<<8;
+    enc1 |= read_byte[4];
+
+    return enc1;
+}
+
+int32_t RoboClaw::ReadEncM2(uint8_t address){
+    int32_t enc2;
+    uint16_t read_byte2[7];
+    write_(address,GETM2ENC,0x00, true,false);
+
+    read_byte2[0] = (uint16_t)_roboclaw.getc();
+    read_byte2[1] = (uint16_t)_roboclaw.getc();
+    read_byte2[2] = (uint16_t)_roboclaw.getc();
+    read_byte2[3] = (uint16_t)_roboclaw.getc();
+    read_byte2[4] = (uint16_t)_roboclaw.getc();
+    read_byte2[5] = (uint16_t)_roboclaw.getc();
+    read_byte2[6] = (uint16_t)_roboclaw.getc();
+
+    enc2 = read_byte2[1]<<24;
+    enc2 |= read_byte2[2]<<16;
+    enc2 |= read_byte2[3]<<8;
+    enc2 |= read_byte2[4];
+
+    return enc2;
+}
+
+int32_t RoboClaw::ReadSpeedM1(uint8_t address){
+    int32_t speed1;
+    uint16_t read_byte[7];
+    write_n(2,address,GETM1SPEED);
+
+    read_byte[0] = (uint16_t)_roboclaw.getc();
+    read_byte[1] = (uint16_t)_roboclaw.getc();
+    read_byte[2] = (uint16_t)_roboclaw.getc();
+    read_byte[3] = (uint16_t)_roboclaw.getc();
+    read_byte[4] = (uint16_t)_roboclaw.getc();
+    read_byte[5] = (uint16_t)_roboclaw.getc();
+    read_byte[6] = (uint16_t)_roboclaw.getc();
+
+    speed1 = read_byte[1]<<24;
+    speed1 |= read_byte[2]<<16;
+    speed1 |= read_byte[3]<<8;
+    speed1 |= read_byte[4];
+
+    return speed1;
+}
+
+int32_t RoboClaw::ReadSpeedM2(uint8_t address){
+    int32_t speed2;
+    uint16_t read_byte2[7];
+    write_n(2,address,GETM2SPEED);
+
+    read_byte2[0] = (uint16_t)_roboclaw.getc();
+    read_byte2[1] = (uint16_t)_roboclaw.getc();
+    read_byte2[2] = (uint16_t)_roboclaw.getc();
+    read_byte2[3] = (uint16_t)_roboclaw.getc();
+    read_byte2[4] = (uint16_t)_roboclaw.getc();
+    read_byte2[5] = (uint16_t)_roboclaw.getc();
+    read_byte2[6] = (uint16_t)_roboclaw.getc();
+
+    speed2 = read_byte2[1]<<24;
+    speed2 |= read_byte2[2]<<16;
+    speed2 |= read_byte2[3]<<8;
+    speed2 |= read_byte2[4];
+
+    return speed2;
+}
+
+void RoboClaw::ResetEnc(uint8_t address){
+    write_n(2,address,RESETENC);
+}
+
+void RoboClaw::SpeedM1(uint8_t address, int32_t speed){
+    write_n(6,address,M1SPEED,SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedM2(uint8_t address, int32_t speed){
+    write_n(6,address,M2SPEED,SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM1(uint8_t address, int32_t accel, int32_t speed){
+    write_n(10,address,M1SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM2(uint8_t address, int32_t accel, int32_t speed){
+    write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel),SetDWORDval(speed));
+}
+
+void RoboClaw::SpeedAccelM1M2(uint8_t address, int32_t accel1, int32_t speed1, int32_t accel2, int32_t speed2){
+    write_n(10,address,M2SPEEDACCEL,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(accel2),SetDWORDval(speed2));
+}
+
+void RoboClaw::SpeedDistanceM1(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(11,address,M1SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedDistanceM2(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(11,address,M2SPEEDDIST,SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDistanceM1(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(15,address,M1SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDistanceM2(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer){
+    write_n(15,address,M2SPEEDACCELDIST,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(distance),buffer);
+}
+
+void RoboClaw::SpeedAccelDeccelPositionM1(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
+    write_n(19,address,M1SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
+}
+
+void RoboClaw::SpeedAccelDeccelPositionM2(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag){
+    write_n(19,address,M2SPEEDACCELDECCELPOS,SetDWORDval(accel),SetDWORDval(speed),SetDWORDval(deccel),SetDWORDval(position),flag);
+}
+
+void RoboClaw::SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag){
+    write_n(35,address,MIXEDSPEEDACCELDECCELPOS,SetDWORDval(accel1),SetDWORDval(speed1),SetDWORDval(deccel1),SetDWORDval(position1),SetDWORDval(accel2),SetDWORDval(speed2),SetDWORDval(deccel2),SetDWORDval(position2),flag);
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RoboClaw.h	Mon Feb 04 04:46:37 2019 +0000
@@ -0,0 +1,108 @@
+#ifndef ROBOCLAW_H
+#define ROBOCLAW_H
+#include "mbed.h"
+#include "registers.h"
+
+/*
+* 一般公開されているライブラリのマイナーチェンジである
+* Serial -> RawSerial
+* 指令時にアドレスを指定できるものを利用した
+* 
+*/
+
+/** The RoboClaw class, yo ucan use the library with : http://www.ionmc.com/RoboClaw-2x15A-Motor-Controller_p_10.html
+*   Used to control one or two motors with (or not) encoders
+*  @code
+* #include "mbed.h"
+* #include "RoboClaw.h"
+*
+* RoboClaw roboclaw(115200, PA_11, PA_12);
+* 
+* int main() {
+*     roboclaw.ForwardM1(ADR, 127);
+*     while(1);
+* }
+* @endcode
+*/
+
+class RoboClaw
+{
+public:
+    /** Create RoboClaw instance
+    */
+    RoboClaw(int baudrate, PinName rx, PinName tx);
+    
+    /** Forward and Backward functions
+    * @param address address of the device
+    * @param speed speed of the motor (between 0 and 127)
+    * @note Forward and Backward functions
+    */
+    void ForwardM1(uint8_t address, int speed);
+    void BackwardM1(uint8_t address, int speed);
+    void ForwardM2(uint8_t address, int speed);
+    void BackwardM2(uint8_t address, int speed);
+    
+    /** Forward and Backward functions
+    * @param address address of the device
+    * @param speed speed of the motor (between 0 and 127)
+    * @note Forward and Backward functions, it turns the two motors
+    */
+    void Forward(uint8_t address, int speed);
+    void Backward(uint8_t address, int speed);
+    
+    /** Read the Firmware
+    * @param address address of the device
+    */
+    void ReadFirm(uint8_t address);
+    
+    /** Read encoder and speed of M1 or M2
+    * @param address address of the device
+    * @note Read encoder in ticks
+    * @note Read speed in ticks per second
+    */
+    int32_t ReadEncM1(uint8_t address);
+    int32_t ReadEncM2(uint8_t address);
+    int32_t ReadSpeedM1(uint8_t address);
+    int32_t ReadSpeedM2(uint8_t address);
+    
+    /** Set both encoders to zero
+    * @param address address of the device
+    */
+    void ResetEnc(uint8_t address);
+    
+    /** Set speed of Motor with different parameter (only in ticks)
+    * @param address address of the device
+    * @note Set the Speed
+    * @note Set the Speed and Accel
+    * @note Set the Speed and Distance
+    * @note Set the Speed, Accel and Distance
+    * @note Set the Speed, Accel, Decceleration and Position
+    */
+    void SpeedM1(uint8_t address, int32_t speed);
+    void SpeedM2(uint8_t address, int32_t speed);
+    void SpeedAccelM1(uint8_t address, int32_t accel, int32_t speed);
+    void SpeedAccelM2(uint8_t address, int32_t accel, int32_t speed);
+    void SpeedAccelM1M2(uint8_t address, int32_t accel1, int32_t speed1, int32_t accel2, int32_t speed2);
+    void SpeedDistanceM1(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedDistanceM2(uint8_t address, int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedAccelDistanceM1(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedAccelDistanceM2(uint8_t address, int32_t accel, int32_t speed, uint32_t distance, uint8_t buffer);
+    void SpeedAccelDeccelPositionM1(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag);
+    void SpeedAccelDeccelPositionM2(uint8_t address, uint32_t accel, int32_t speed, uint32_t deccel, int32_t position, uint8_t flag);
+    void SpeedAccelDeccelPositionM1M2(uint8_t address,uint32_t accel1,uint32_t speed1,uint32_t deccel1, int32_t position1,uint32_t accel2,uint32_t speed2,uint32_t deccel2, int32_t position2,uint8_t flag);
+    
+private:
+    RawSerial _roboclaw;
+    uint16_t crc;
+    void crc_clear();
+    void crc_update(uint8_t data);
+    uint16_t crc_get();
+    
+    void write_n(uint8_t cnt, ...);
+    void write_(uint8_t address, uint8_t command, uint8_t data, bool reading, bool crcon);
+    
+    uint16_t crc16(uint8_t *packet, int nBytes);
+    uint8_t read_(void);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/registers.h	Mon Feb 04 04:46:37 2019 +0000
@@ -0,0 +1,87 @@
+#define ADR 0x80
+#define M1FORWARD 0
+#define M1BACKWARD 1
+#define SETMINMB 2
+#define SETMAXMB 3
+#define M2FORWARD 4
+#define M2BACKWARD 5
+#define M17BIT 6
+#define M27BIT 7
+#define MIXEDFORWARD 8
+#define MIXEDBACKWARD 9
+#define MIXEDRIGHT 10
+#define MIXEDLEFT 11
+#define MIXEDFB 12
+#define MIXEDLR 13
+#define GETM1ENC 16
+#define GETM2ENC 17
+#define GETM1SPEED 18
+#define GETM2SPEED 19
+#define RESETENC 20
+#define GETVERSION 21
+#define SETM1ENCCOUNT  22
+#define SETM2ENCCOUNT  23
+#define GETMBATT  24
+#define GETLBATT  25
+#define SETMINLB  26
+#define SETMAXLB  27
+#define SETM1PID  28
+#define SETM2PID  29
+#define GETM1ISPEED  30
+#define GETM2ISPEED  31
+#define M1DUTY  32
+#define M2DUTY  33
+#define MIXEDDUTY  34
+#define M1SPEED  35
+#define M2SPEED  36
+#define MIXEDSPEED  37
+#define M1SPEEDACCEL  38
+#define M2SPEEDACCEL  39
+#define MIXEDSPEEDACCEL  40
+#define M1SPEEDDIST  41
+#define M2SPEEDDIST  42
+#define MIXEDSPEEDDIST  43
+#define M1SPEEDACCELDIST  44
+#define M2SPEEDACCELDIST  45
+#define MIXEDSPEEDACCELDIST  46
+#define GETBUFFERS  47
+#define GETCURRENTS  49
+#define MIXEDSPEED2ACCEL  50
+#define MIXEDSPEED2ACCELDIST  51
+#define M1DUTYACCEL  52
+#define M2DUTYACCEL  53
+#define MIXEDDUTYACCEL  54
+#define READM1PID  55
+#define READM2PID  56
+#define SETMAINVOLTAGES  57
+#define SETLOGICVOLTAGES  58
+#define GETMINMAXMAINVOLTAGES  59
+#define GETMINMAXLOGICVOLTAGES  60
+#define SETM1POSPID  61
+#define SETM2POSPID  62
+#define READM1POSPID  63
+#define READM2POSPID  64
+#define M1SPEEDACCELDECCELPOS  65
+#define M2SPEEDACCELDECCELPOS  66
+#define MIXEDSPEEDACCELDECCELPOS  67
+#define SETM1DEFAULTACCEL  68
+#define SETM2DEFAULTACCEL  69
+#define SETPINFUNCTIONS  74
+#define GETPINFUNCTIONS  75
+#define RESTOREDEFAULTS  80
+#define GETTEMP  82
+#define GETTEMP2  83
+#define GETERROR  90
+#define GETENCODERMODE  91
+#define SETM1ENCODERMODE  92
+#define SETM2ENCODERMODE  93
+#define WRITENVM  94
+#define READNVM  95
+#define SETCONFIG  98
+#define GETCONFIG  99
+#define SETM1MAXCURRENT  133
+#define SETM2MAXCURRENT  134
+#define GETM1MAXCURRENT  135
+#define GETM2MAXCURRENT  136
+#define SETPWMMODE  148
+#define GETPWMMODE 149
\ No newline at end of file