Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Eigen

shared/I2CMD/I2CMD.cpp@0:ee7e9405e1c7, 2021-04-14 (annotated)

Committer:
e2011220
Date:
Wed Apr 14 07:26:19 2021 +0000
Revision:
0:ee7e9405e1c7
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e2011220 0:ee7e9405e1c7 1
e2011220 0:ee7e9405e1c7 2 #include <math.h>
e2011220 0:ee7e9405e1c7 3 #include <shared/I2CMD/I2CMD.h>
e2011220 0:ee7e9405e1c7 4
e2011220 0:ee7e9405e1c7 5 I2CMD::I2CMD(I2C *i2c_bus, int i2c_address, int motor_id, int retries)
e2011220 0:ee7e9405e1c7 6 : _i2c_bus(i2c_bus),_i2c_address(i2c_address),_motor_id(motor_id)
e2011220 0:ee7e9405e1c7 7 , _retries(retries), _error(true)
e2011220 0:ee7e9405e1c7 8 {
e2011220 0:ee7e9405e1c7 9 }
e2011220 0:ee7e9405e1c7 10
e2011220 0:ee7e9405e1c7 11 void I2CMD::drive(double strength)
e2011220 0:ee7e9405e1c7 12 {
e2011220 0:ee7e9405e1c7 13 uint8_t dir;
e2011220 0:ee7e9405e1c7 14 char data[2];
e2011220 0:ee7e9405e1c7 15
e2011220 0:ee7e9405e1c7 16 if(signbit(strength)) {
e2011220 0:ee7e9405e1c7 17 dir = 1;
e2011220 0:ee7e9405e1c7 18 } else {
e2011220 0:ee7e9405e1c7 19 dir = 0;
e2011220 0:ee7e9405e1c7 20 }
e2011220 0:ee7e9405e1c7 21
e2011220 0:ee7e9405e1c7 22 data[0] = dir;
e2011220 0:ee7e9405e1c7 23 data[1] = conv_strength_to_duty_u8(strength);
e2011220 0:ee7e9405e1c7 24
e2011220 0:ee7e9405e1c7 25 i2c_write('D', data, 2);
e2011220 0:ee7e9405e1c7 26 }
e2011220 0:ee7e9405e1c7 27
e2011220 0:ee7e9405e1c7 28 void I2CMD::brake(double strength)
e2011220 0:ee7e9405e1c7 29 {
e2011220 0:ee7e9405e1c7 30 char data[1];
e2011220 0:ee7e9405e1c7 31 data[0] = conv_strength_to_duty_u8(strength);
e2011220 0:ee7e9405e1c7 32 i2c_write('B', data, 1);
e2011220 0:ee7e9405e1c7 33 }
e2011220 0:ee7e9405e1c7 34
e2011220 0:ee7e9405e1c7 35 void I2CMD::free()
e2011220 0:ee7e9405e1c7 36 {
e2011220 0:ee7e9405e1c7 37 char data[1] = {0};
e2011220 0:ee7e9405e1c7 38 i2c_write('F', data, 1);
e2011220 0:ee7e9405e1c7 39 }
e2011220 0:ee7e9405e1c7 40
e2011220 0:ee7e9405e1c7 41 void I2CMD::set_retries(int retries)
e2011220 0:ee7e9405e1c7 42 {
e2011220 0:ee7e9405e1c7 43 _retries = retries;
e2011220 0:ee7e9405e1c7 44 }
e2011220 0:ee7e9405e1c7 45
e2011220 0:ee7e9405e1c7 46 bool I2CMD::get_error()
e2011220 0:ee7e9405e1c7 47 {
e2011220 0:ee7e9405e1c7 48 return _error;
e2011220 0:ee7e9405e1c7 49 }
e2011220 0:ee7e9405e1c7 50
e2011220 0:ee7e9405e1c7 51 bool I2CMD::i2c_write(int command, char *data, int length)
e2011220 0:ee7e9405e1c7 52 {
e2011220 0:ee7e9405e1c7 53 const int send_length = 2 + length;
e2011220 0:ee7e9405e1c7 54 char send_data[send_length];
e2011220 0:ee7e9405e1c7 55 bool error = true;
e2011220 0:ee7e9405e1c7 56
e2011220 0:ee7e9405e1c7 57 send_data[0] = _motor_id;
e2011220 0:ee7e9405e1c7 58 send_data[1] = command;
e2011220 0:ee7e9405e1c7 59 for(int i = 0; i < length; i++) {
e2011220 0:ee7e9405e1c7 60 send_data[i + 2] = data[i];
e2011220 0:ee7e9405e1c7 61 }
e2011220 0:ee7e9405e1c7 62
e2011220 0:ee7e9405e1c7 63 for(int i = 0; i < _retries && error; i++) {
e2011220 0:ee7e9405e1c7 64 error = _i2c_bus->write(_i2c_address, send_data, send_length);
e2011220 0:ee7e9405e1c7 65 }
e2011220 0:ee7e9405e1c7 66
e2011220 0:ee7e9405e1c7 67 _error = error;
e2011220 0:ee7e9405e1c7 68 return error;
e2011220 0:ee7e9405e1c7 69 }
e2011220 0:ee7e9405e1c7 70
e2011220 0:ee7e9405e1c7 71 uint8_t I2CMD::conv_strength_to_duty_u8(double strength)
e2011220 0:ee7e9405e1c7 72 {
e2011220 0:ee7e9405e1c7 73 double duty;
e2011220 0:ee7e9405e1c7 74 uint8_t duty_u8;
e2011220 0:ee7e9405e1c7 75
e2011220 0:ee7e9405e1c7 76 duty = fabs(strength);
e2011220 0:ee7e9405e1c7 77 if(duty > 0.3) {
e2011220 0:ee7e9405e1c7 78 duty = 0.3;
e2011220 0:ee7e9405e1c7 79 }
e2011220 0:ee7e9405e1c7 80 duty_u8 = floor(duty * 255);
e2011220 0:ee7e9405e1c7 81
e2011220 0:ee7e9405e1c7 82 return duty_u8;
e2011220 0:ee7e9405e1c7 83 }