Baijun Desai / Mbed 2 deprecated MultiModalRobotSM

Dependencies:   Motordriver mbed-rtos mbed

Fork of MultiModalRobotSM by Hemanth Koralla

MultiModalRobot/MultiModalRobot.cpp

Committer:
baijun
Date:
2017-12-12
Revision:
21:21f72bd7f649
Parent:
20:e1a78ee68726

File content as of revision 21:21f72bd7f649:

#include "MultiModalRobot.h"

MultiModalRobot::MultiModalRobot(Motor& left, Motor& right) : leftWheel(left), rightWheel(right), leftDir(0), rightDir(0) {}



void MultiModalRobot::driveWheels(float leftSpeed, float rightSpeed){
    if(leftSpeed*leftDir<0 || rightSpeed*rightDir<0){
     stop(0.5);
    }
    if(leftSpeed<0){
        leftDir = -1;
    } else {
        leftDir = 1;
    }
    if(rightSpeed<0){
        rightDir = -1;
    } else {
        rightDir = 1;
    }
    leftWheel.speed(leftSpeed);
    rightWheel.speed(rightSpeed);
    wait(0.1);
    stop(0.5);
}

void MultiModalRobot::stop(float dutyCycle){
    leftDir = rightDir = 0;
    leftWheel.coast();
    rightWheel.coast();
}