Takeuchi Issei / Mbed 2 deprecated 4Omunisleeve3

メカナムのコード

Dependencies:   mbed Eigen

shared/I2CMDMain/I2CMDMain.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 #include "I2CMDMain.h"
e2011220 0:ee7e9405e1c7 2
e2011220 0:ee7e9405e1c7 3 #include <math.h>
e2011220 0:ee7e9405e1c7 4
e2011220 0:ee7e9405e1c7 5 I2CMDMain::I2CMDMain(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 for(int i = 0; i < 4; i++)
e2011220 0:ee7e9405e1c7 10 send_data[i] = 0;
e2011220 0:ee7e9405e1c7 11 send_flg = 0;
e2011220 0:ee7e9405e1c7 12 send_length = 0;
e2011220 0:ee7e9405e1c7 13 }
e2011220 0:ee7e9405e1c7 14
e2011220 0:ee7e9405e1c7 15 void I2CMDMain::drive(double strength)
e2011220 0:ee7e9405e1c7 16 {
e2011220 0:ee7e9405e1c7 17 uint8_t dir;
e2011220 0:ee7e9405e1c7 18 char data[2];
e2011220 0:ee7e9405e1c7 19
e2011220 0:ee7e9405e1c7 20 if(signbit(strength)) {
e2011220 0:ee7e9405e1c7 21 dir = 1;
e2011220 0:ee7e9405e1c7 22 } else {
e2011220 0:ee7e9405e1c7 23 dir = 0;
e2011220 0:ee7e9405e1c7 24 }
e2011220 0:ee7e9405e1c7 25
e2011220 0:ee7e9405e1c7 26 data[0] = dir;
e2011220 0:ee7e9405e1c7 27 data[1] = conv_strength_to_duty_u8(strength);
e2011220 0:ee7e9405e1c7 28
e2011220 0:ee7e9405e1c7 29 i2c_write('D', data, 2);
e2011220 0:ee7e9405e1c7 30 }
e2011220 0:ee7e9405e1c7 31
e2011220 0:ee7e9405e1c7 32 void I2CMDMain::brake(double strength)
e2011220 0:ee7e9405e1c7 33 {
e2011220 0:ee7e9405e1c7 34 char data[1];
e2011220 0:ee7e9405e1c7 35 data[0] = conv_strength_to_duty_u8(strength);
e2011220 0:ee7e9405e1c7 36 i2c_write('B', data, 1);
e2011220 0:ee7e9405e1c7 37 }
e2011220 0:ee7e9405e1c7 38
e2011220 0:ee7e9405e1c7 39 void I2CMDMain::free()
e2011220 0:ee7e9405e1c7 40 {
e2011220 0:ee7e9405e1c7 41 char data[1] = {0};
e2011220 0:ee7e9405e1c7 42 i2c_write('F', data, 1);
e2011220 0:ee7e9405e1c7 43 }
e2011220 0:ee7e9405e1c7 44
e2011220 0:ee7e9405e1c7 45 void I2CMDMain::set_retries(int retries)
e2011220 0:ee7e9405e1c7 46 {
e2011220 0:ee7e9405e1c7 47 _retries = retries;
e2011220 0:ee7e9405e1c7 48 }
e2011220 0:ee7e9405e1c7 49
e2011220 0:ee7e9405e1c7 50 bool I2CMDMain::get_error()
e2011220 0:ee7e9405e1c7 51 {
e2011220 0:ee7e9405e1c7 52 return _error;
e2011220 0:ee7e9405e1c7 53 }
e2011220 0:ee7e9405e1c7 54
e2011220 0:ee7e9405e1c7 55 bool I2CMDMain::i2c_send()
e2011220 0:ee7e9405e1c7 56 {
e2011220 0:ee7e9405e1c7 57 bool error = true;
e2011220 0:ee7e9405e1c7 58 if(send_flg){
e2011220 0:ee7e9405e1c7 59 for(int i = 0; i < _retries && error; i++) {
e2011220 0:ee7e9405e1c7 60 error = _i2c_bus->write(_i2c_address, send_data, send_length);
e2011220 0:ee7e9405e1c7 61 }
e2011220 0:ee7e9405e1c7 62 _error = error;
e2011220 0:ee7e9405e1c7 63 send_flg = 0;
e2011220 0:ee7e9405e1c7 64 }
e2011220 0:ee7e9405e1c7 65 wait(0.0001);
e2011220 0:ee7e9405e1c7 66 return error;
e2011220 0:ee7e9405e1c7 67 }
e2011220 0:ee7e9405e1c7 68
e2011220 0:ee7e9405e1c7 69 bool I2CMDMain::i2c_write(int command, char *data, int length)
e2011220 0:ee7e9405e1c7 70 {
e2011220 0:ee7e9405e1c7 71 send_length = 2 + length;
e2011220 0:ee7e9405e1c7 72 int error = 0;
e2011220 0:ee7e9405e1c7 73
e2011220 0:ee7e9405e1c7 74 send_data[0] = _motor_id;
e2011220 0:ee7e9405e1c7 75 send_data[1] = command;
e2011220 0:ee7e9405e1c7 76 for(int i = 0; i < length; i++) {
e2011220 0:ee7e9405e1c7 77 send_data[i + 2] = data[i];
e2011220 0:ee7e9405e1c7 78 }
e2011220 0:ee7e9405e1c7 79 send_flg = 1;
e2011220 0:ee7e9405e1c7 80
e2011220 0:ee7e9405e1c7 81 return (bool)error;
e2011220 0:ee7e9405e1c7 82 }
e2011220 0:ee7e9405e1c7 83
e2011220 0:ee7e9405e1c7 84 uint8_t I2CMDMain::conv_strength_to_duty_u8(double strength)
e2011220 0:ee7e9405e1c7 85 {
e2011220 0:ee7e9405e1c7 86 double duty;
e2011220 0:ee7e9405e1c7 87 uint8_t duty_u8;
e2011220 0:ee7e9405e1c7 88
e2011220 0:ee7e9405e1c7 89 duty = fabs(strength);
e2011220 0:ee7e9405e1c7 90 if(duty > 1.0) {
e2011220 0:ee7e9405e1c7 91 duty = 1.0;
e2011220 0:ee7e9405e1c7 92 }
e2011220 0:ee7e9405e1c7 93 duty_u8 = floor(duty * 255);
e2011220 0:ee7e9405e1c7 94
e2011220 0:ee7e9405e1c7 95 return duty_u8;
e2011220 0:ee7e9405e1c7 96 }