Mark Vandermeulen / FYDP_Final

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

Dependencies:   mbed

motors/motors.cpp

Committer:
majik
Date:
2015-03-18
Revision:
0:964eb6a2ef00

File content as of revision 0:964eb6a2ef00:

#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.