Controller firmware for a mobile robot, having a K64F MCU on board. Please read README.md for details.
Diff: drive.cpp
- Revision:
- 0:260ca1f1cba7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/drive.cpp Wed May 30 15:10:20 2018 +0000 @@ -0,0 +1,71 @@ +#include "drive.h" + +Drive::Drive() {} +Drive::Drive(PinName pF, PinName pB, PinName pL, PinName pR, Lights* lights) { + // create Pwm and DigitalOut objects + po_forward = new PwmOut(pF); + po_backward = new PwmOut(pB); + do_steerLeft = new DigitalOut(pL); + do_steerRight = new DigitalOut(pR); + + this->lights = lights; + autoIndex = (lights)?true:false; + + po_forward->period(0.0001); // 10kHz + po_backward->period(0.0001); // 10kHz + + printf("Drive created\n"); +} + +Drive::~Drive() { + delete po_forward; + delete po_backward; + delete do_steerLeft; + delete do_steerRight; +} + +void Drive::forward() { + if(lights) + lights->reversingLightOff(); + po_backward->write(0.0f); + po_forward->write(0.85f); +} + +void Drive::backward() { + if(lights) + lights->reversingLightOn(); + po_forward->write(0.0f); + po_backward->write(0.99f); +} + +void Drive::stop() { + if(lights) + lights->reversingLightOff(); + po_forward->write(0.0f); + po_backward->write(0.0f); +} + +void Drive::steerLeft() { + if(lights && autoIndex) + lights->indexLeft(); + do_steerRight->write(0); + do_steerLeft->write(1); +} + +void Drive::steerRight() { + if(lights && autoIndex) + lights->indexRight(); + do_steerLeft->write(0); + do_steerRight->write(1); +} + +void Drive::steerStraight() { + if(lights && autoIndex) + lights->indexOff(); + do_steerLeft->write(0); + do_steerRight->write(0); +} + +void Drive::setAutoIndex(bool autoIndex) { + this->autoIndex = autoIndex; +}