ken fuji / T_motor

田中くんのモタドラを制御する石本さんのやつ

T_motor.cpp@2:f82a7c0b697f, 2017-08-21 (annotated)

Committer:
fujikenac
Date:
Mon Aug 21 00:56:29 2017 +0000
Revision:
2:f82a7c0b697f
Parent:
1:5bd161b83ce0
Child:
3:a3be8ae9fed1
add sqrt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fujikenac 0:e10d08530490 1 #include "mbed.h"
fujikenac 0:e10d08530490 2 #include "T_motor.h"
fujikenac 0:e10d08530490 3
fujikenac 0:e10d08530490 4 void T_motor::setAddr(int addr_)
fujikenac 0:e10d08530490 5 {
fujikenac 0:e10d08530490 6 addr = addr_;
fujikenac 0:e10d08530490 7 }
fujikenac 0:e10d08530490 8
fujikenac 0:e10d08530490 9 void T_motor::init(char addr,int freq = 100000)
fujikenac 0:e10d08530490 10 {
fujikenac 0:e10d08530490 11 setAddr(addr);
fujikenac 0:e10d08530490 12 i2c.frequency(freq);
fujikenac 0:e10d08530490 13 char d = 0;
fujikenac 0:e10d08530490 14 i2c.write((addr+1) << 1,&d,1);
fujikenac 0:e10d08530490 15 wait(0.01);
fujikenac 0:e10d08530490 16 }
fujikenac 0:e10d08530490 17
fujikenac 0:e10d08530490 18 T_motor::T_motor(PinName sda,PinName scl,int addr):i2c(sda,scl)
fujikenac 0:e10d08530490 19 {
fujikenac 0:e10d08530490 20 init(addr);
fujikenac 0:e10d08530490 21 }
fujikenac 0:e10d08530490 22
fujikenac 0:e10d08530490 23 T_motor::T_motor(I2C& i2c_,int addr):i2c(i2c_)
fujikenac 0:e10d08530490 24 {
fujikenac 0:e10d08530490 25 init(addr);
fujikenac 0:e10d08530490 26 }
fujikenac 0:e10d08530490 27
fujikenac 1:5bd161b83ce0 28 T_motor& T_motor::operator=(float fval)// -1 <= fval <= 1
fujikenac 0:e10d08530490 29 {
fujikenac 2:f82a7c0b697f 30 fval = sqrt(fval);
fujikenac 0:e10d08530490 31 if(fabs(fval) < 0.1f)free();
fujikenac 0:e10d08530490 32 else {
fujikenac 0:e10d08530490 33 if(fval < 0) run(1,char(-1*fval*255));
fujikenac 0:e10d08530490 34 else if(fval > 0) run(0,char(fval*255));
fujikenac 0:e10d08530490 35 }
fujikenac 0:e10d08530490 36 return *this;
fujikenac 0:e10d08530490 37 }
fujikenac 0:e10d08530490 38
fujikenac 1:5bd161b83ce0 39 void T_motor::control(float fval)//copy from operator
fujikenac 1:5bd161b83ce0 40 {
fujikenac 2:f82a7c0b697f 41 fval = sqrt(fval);
fujikenac 1:5bd161b83ce0 42 if(fabs(fval) < 0.1f)free();
fujikenac 1:5bd161b83ce0 43 else {
fujikenac 1:5bd161b83ce0 44 if(fval < 0) run(1,char(-1*fval*255));
fujikenac 1:5bd161b83ce0 45 else if(fval > 0) run(0,char(fval*255));
fujikenac 1:5bd161b83ce0 46 }
fujikenac 1:5bd161b83ce0 47 }
fujikenac 1:5bd161b83ce0 48
fujikenac 0:e10d08530490 49 bool T_motor::run(char mode,char speed)
fujikenac 0:e10d08530490 50 {
fujikenac 0:e10d08530490 51 bool f = 0;
fujikenac 0:e10d08530490 52 char d = speed;
fujikenac 0:e10d08530490 53
fujikenac 0:e10d08530490 54 if(mode == 0 || mode == 1)f = i2c.write((addr+mode) << 1,&d,1);
fujikenac 0:e10d08530490 55 wait(0.01);
fujikenac 0:e10d08530490 56 return f;
fujikenac 0:e10d08530490 57 }
fujikenac 0:e10d08530490 58
fujikenac 0:e10d08530490 59 bool T_motor::stop()
fujikenac 0:e10d08530490 60 {
fujikenac 0:e10d08530490 61 return run(1,0);
fujikenac 0:e10d08530490 62 }
fujikenac 0:e10d08530490 63
fujikenac 0:e10d08530490 64 bool T_motor::free()
fujikenac 0:e10d08530490 65 {
fujikenac 0:e10d08530490 66 return run(0,0);
fujikenac 0:e10d08530490 67 }
fujikenac 0:e10d08530490 68