Szilárd Greszler / robotkocsi

Controller firmware for a mobile robot, having a K64F MCU on board. Please read README.md for details.

Dependents:   robotkocsi_OS

drive.cpp@0:260ca1f1cba7, 2018-05-30 (annotated)

Committer:
dralisz82
Date:
Wed May 30 15:10:20 2018 +0000
Revision:
0:260ca1f1cba7
Controller firmware for a mobile robot, having a K64F MCU on board.; ; See README.md for details;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dralisz82 0:260ca1f1cba7 1 #include "drive.h"
dralisz82 0:260ca1f1cba7 2
dralisz82 0:260ca1f1cba7 3 Drive::Drive() {}
dralisz82 0:260ca1f1cba7 4 Drive::Drive(PinName pF, PinName pB, PinName pL, PinName pR, Lights* lights) {
dralisz82 0:260ca1f1cba7 5 // create Pwm and DigitalOut objects
dralisz82 0:260ca1f1cba7 6 po_forward = new PwmOut(pF);
dralisz82 0:260ca1f1cba7 7 po_backward = new PwmOut(pB);
dralisz82 0:260ca1f1cba7 8 do_steerLeft = new DigitalOut(pL);
dralisz82 0:260ca1f1cba7 9 do_steerRight = new DigitalOut(pR);
dralisz82 0:260ca1f1cba7 10
dralisz82 0:260ca1f1cba7 11 this->lights = lights;
dralisz82 0:260ca1f1cba7 12 autoIndex = (lights)?true:false;
dralisz82 0:260ca1f1cba7 13
dralisz82 0:260ca1f1cba7 14 po_forward->period(0.0001); // 10kHz
dralisz82 0:260ca1f1cba7 15 po_backward->period(0.0001); // 10kHz
dralisz82 0:260ca1f1cba7 16
dralisz82 0:260ca1f1cba7 17 printf("Drive created\n");
dralisz82 0:260ca1f1cba7 18 }
dralisz82 0:260ca1f1cba7 19
dralisz82 0:260ca1f1cba7 20 Drive::~Drive() {
dralisz82 0:260ca1f1cba7 21 delete po_forward;
dralisz82 0:260ca1f1cba7 22 delete po_backward;
dralisz82 0:260ca1f1cba7 23 delete do_steerLeft;
dralisz82 0:260ca1f1cba7 24 delete do_steerRight;
dralisz82 0:260ca1f1cba7 25 }
dralisz82 0:260ca1f1cba7 26
dralisz82 0:260ca1f1cba7 27 void Drive::forward() {
dralisz82 0:260ca1f1cba7 28 if(lights)
dralisz82 0:260ca1f1cba7 29 lights->reversingLightOff();
dralisz82 0:260ca1f1cba7 30 po_backward->write(0.0f);
dralisz82 0:260ca1f1cba7 31 po_forward->write(0.85f);
dralisz82 0:260ca1f1cba7 32 }
dralisz82 0:260ca1f1cba7 33
dralisz82 0:260ca1f1cba7 34 void Drive::backward() {
dralisz82 0:260ca1f1cba7 35 if(lights)
dralisz82 0:260ca1f1cba7 36 lights->reversingLightOn();
dralisz82 0:260ca1f1cba7 37 po_forward->write(0.0f);
dralisz82 0:260ca1f1cba7 38 po_backward->write(0.99f);
dralisz82 0:260ca1f1cba7 39 }
dralisz82 0:260ca1f1cba7 40
dralisz82 0:260ca1f1cba7 41 void Drive::stop() {
dralisz82 0:260ca1f1cba7 42 if(lights)
dralisz82 0:260ca1f1cba7 43 lights->reversingLightOff();
dralisz82 0:260ca1f1cba7 44 po_forward->write(0.0f);
dralisz82 0:260ca1f1cba7 45 po_backward->write(0.0f);
dralisz82 0:260ca1f1cba7 46 }
dralisz82 0:260ca1f1cba7 47
dralisz82 0:260ca1f1cba7 48 void Drive::steerLeft() {
dralisz82 0:260ca1f1cba7 49 if(lights && autoIndex)
dralisz82 0:260ca1f1cba7 50 lights->indexLeft();
dralisz82 0:260ca1f1cba7 51 do_steerRight->write(0);
dralisz82 0:260ca1f1cba7 52 do_steerLeft->write(1);
dralisz82 0:260ca1f1cba7 53 }
dralisz82 0:260ca1f1cba7 54
dralisz82 0:260ca1f1cba7 55 void Drive::steerRight() {
dralisz82 0:260ca1f1cba7 56 if(lights && autoIndex)
dralisz82 0:260ca1f1cba7 57 lights->indexRight();
dralisz82 0:260ca1f1cba7 58 do_steerLeft->write(0);
dralisz82 0:260ca1f1cba7 59 do_steerRight->write(1);
dralisz82 0:260ca1f1cba7 60 }
dralisz82 0:260ca1f1cba7 61
dralisz82 0:260ca1f1cba7 62 void Drive::steerStraight() {
dralisz82 0:260ca1f1cba7 63 if(lights && autoIndex)
dralisz82 0:260ca1f1cba7 64 lights->indexOff();
dralisz82 0:260ca1f1cba7 65 do_steerLeft->write(0);
dralisz82 0:260ca1f1cba7 66 do_steerRight->write(0);
dralisz82 0:260ca1f1cba7 67 }
dralisz82 0:260ca1f1cba7 68
dralisz82 0:260ca1f1cba7 69 void Drive::setAutoIndex(bool autoIndex) {
dralisz82 0:260ca1f1cba7 70 this->autoIndex = autoIndex;
dralisz82 0:260ca1f1cba7 71 }