Dave Lu / FYDP_Final2

Only imu output

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

motors/motors.cpp

Committer:
tntmarket
Date:
2015-03-25
Revision:
5:d2e955a94940
Parent:
0:21019d94ad33

File content as of revision 5:d2e955a94940:

#include "motors.h"

Motors::Motors(TB6612 * L, TB6612 * R, PinName STBY):
    Left(L), Right(R), enable(STBY)
{
    enable = 1;
}

void Motors::flip()
{
    TB6612 * temp = Left;
    Left = Right;
    Right = temp;

    Left->scale *= -1;
    Right->scale *= -1;              
}    
void Motors::reverse(bool m)
{
    if(m)
        Right->scale *= -1; //reverse right motor
    else
        Left->scale *= -1;  //reverse left motor
}

//TODO: write in a function that automatically sets the motors to be oriented properly. Use the
//gyroscopes and accelerometer to do this.