Dave Lu / FYDP_Final2

Only imu output

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

drivers/TB6612FNG2/TB6612.cpp@5:d2e955a94940, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 09:58:50 2015 +0000
Revision:
5:d2e955a94940
Parent:
0:21019d94ad33 
only imu output

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:21019d94ad33 1 /**
majik 0:21019d94ad33 2 * Motor Driver TB6612 Control Library
majik 0:21019d94ad33 3 *
majik 0:21019d94ad33 4 * -- TB6612 is a device of the TOSHIBA.
majik 0:21019d94ad33 5 *
majik 0:21019d94ad33 6 * Copyright (C) 2012 Junichi Katsu (JKSOFT)
majik 0:21019d94ad33 7 */
majik 0:21019d94ad33 8
majik 0:21019d94ad33 9
majik 0:21019d94ad33 10 #include "TB6612.h"
majik 0:21019d94ad33 11
majik 0:21019d94ad33 12 // TB6612 Class Constructor
majik 0:21019d94ad33 13 TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
majik 0:21019d94ad33 14 scale(1), _pwm(pwm), _fwd(fwd), _rev(rev) {
majik 0:21019d94ad33 15
majik 0:21019d94ad33 16 _fwd = 0;
majik 0:21019d94ad33 17 _rev = 0;
majik 0:21019d94ad33 18 _pwm = 0.0;
majik 0:21019d94ad33 19 _pwm.period(0.001);
majik 0:21019d94ad33 20 }
majik 0:21019d94ad33 21
majik 0:21019d94ad33 22 // Speed Control
majik 0:21019d94ad33 23 // arg
majik 0:21019d94ad33 24 // int speed -100 -- 0 -- 100
majik 0:21019d94ad33 25 void TB6612::speed(int speed_) {
majik 0:21019d94ad33 26
majik 0:21019d94ad33 27 float speed = scale*(float)speed_;
majik 0:21019d94ad33 28
majik 0:21019d94ad33 29 if( speed > 0 )
majik 0:21019d94ad33 30 {
majik 0:21019d94ad33 31 _pwm = ((float)speed)/ 100.0;
majik 0:21019d94ad33 32 _fwd = 1;
majik 0:21019d94ad33 33 _rev = 0;
majik 0:21019d94ad33 34 }
majik 0:21019d94ad33 35 else if( speed < 0 )
majik 0:21019d94ad33 36 {
majik 0:21019d94ad33 37 _pwm = -((float)speed)/ 100.0;
majik 0:21019d94ad33 38 _fwd = 0;
majik 0:21019d94ad33 39 _rev = 1;
majik 0:21019d94ad33 40 }
majik 0:21019d94ad33 41 else
majik 0:21019d94ad33 42 {
majik 0:21019d94ad33 43 _fwd = 1;
majik 0:21019d94ad33 44 _rev = 1;
majik 0:21019d94ad33 45 }
majik 0:21019d94ad33 46 }