first_library

Dependents:   2019_tourobo_upper minirobo_upper_reserve minirobo_under_reserve serial_RTX_NUCLEA

Files at this revision

API Documentation at this revision

Comitter:
sink
Date:
Mon Feb 04 04:46:37 2019 +0000
Parent:
0:03e99bd9339e
Commit message:
ok

Changed in this revision

RoboClaw.cpp Show annotated file Show diff for this revision Revisions of this file
RoboClaw.h Show annotated file Show diff for this revision Revisions of this file
registers.h Show annotated file Show diff for this revision Revisions of this file
diff -r 03e99bd9339e -r 9dd052490f0a RoboClaw.cpp
--- /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);
+}
+
diff -r 03e99bd9339e -r 9dd052490f0a RoboClaw.h
--- /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
diff -r 03e99bd9339e -r 9dd052490f0a registers.h
--- /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