Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

drivers/TB6612FNG2/TB6612.cpp@0:964eb6a2ef00, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:23:48 2015 +0000
Revision:
0:964eb6a2ef00
This is our FYDP code, but only one IMU works with the RTOS.

Who changed what in which revision?

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