Controller firmware for a mobile robot, having a K64F MCU on board. Please read README.md for details.
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?
User | Revision | Line number | New 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 | } |