Mateusz Grzywacz / Yellow2WheeledRobot_motor_shield

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

YellowMotors.cpp@4:aef786c67b2d, 2016-02-05 (annotated)

Committer:
amateusz
Date:
Fri Feb 05 15:49:11 2016 +0000
Revision:
4:aef786c67b2d
Parent:
3:c656543830df
Child:
5:6dbb019203e9
added option to attach a digitalout (led) to the class for debuging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amateusz 0:85e85976c650 1 #include <math.h>
amateusz 2:6f6e591f1838 2 #include "YellowMotors.h"
amateusz 2:6f6e591f1838 3 #include "mbed.h"
amateusz 2:6f6e591f1838 4
amateusz 0:85e85976c650 5
amateusz 0:85e85976c650 6 float motorLinearizationL(float desired)
amateusz 0:85e85976c650 7 {
amateusz 4:aef786c67b2d 8 return (float)(((6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0)*(desired*10/9+0.1));
amateusz 2:6f6e591f1838 9 };
amateusz 0:85e85976c650 10
amateusz 0:85e85976c650 11 float motorLinearizationR(float desired)
amateusz 0:85e85976c650 12 {
amateusz 4:aef786c67b2d 13 return (float)(4.096509*(exp(0.0286952296*(desired*100))+5.073644964)/100.0*(desired*10/9+0.1)); // \/ history of trial and error
amateusz 0:85e85976c650 14 //return 5.9693939*(exp(0.0251906*(desired*100))+3.162519)/100.0; // that wasn't that bad at all! to early start, the rest ok
amateusz 0:85e85976c650 15 //return (3.45183*(exp(0.0292461*(desired*100))+5.51727))/100.0;
amateusz 2:6f6e591f1838 16 };
amateusz 2:6f6e591f1838 17
amateusz 3:c656543830df 18 YellowMotors::YellowMotors(PinName clk, PinName lat, PinName dat, PinName ena, PinName Lpwm, PinName Rpwm) : _clk(clk), _lat(lat), _dat(dat), _ena(ena), L(D11), R(D3) //, _Lpwm(Lpwm), _Rpwm(Rpwm)
amateusz 2:6f6e591f1838 19 {
amateusz 4:aef786c67b2d 20 _init();
amateusz 4:aef786c67b2d 21 }
amateusz 4:aef786c67b2d 22 void YellowMotors::_init()
amateusz 4:aef786c67b2d 23 {
amateusz 3:c656543830df 24 _directions = 0xff;
amateusz 2:6f6e591f1838 25 //PwmOut _Lpwm(D11);
amateusz 2:6f6e591f1838 26 //PwmOut _Rpwm(D3);
amateusz 2:6f6e591f1838 27 //DigitalOut _dat(dat);
amateusz 2:6f6e591f1838 28 //DigitalOut _clk(clk);
amateusz 2:6f6e591f1838 29 //DigitalOut _lat(lat);
amateusz 2:6f6e591f1838 30
amateusz 3:c656543830df 31 //motorItself::motorItself L(Lpwm);
amateusz 3:c656543830df 32 //motorItself::motorItself R(Rpwm);
amateusz 2:6f6e591f1838 33 // and create motor objects
amateusz 2:6f6e591f1838 34 _ena.write(0);
amateusz 2:6f6e591f1838 35 //pc.printf("konstruktor yellow. Lpwm: %d\n\r", Lpwm);
amateusz 2:6f6e591f1838 36 }
amateusz 3:c656543830df 37 void YellowMotors::operator= (const float value)
amateusz 3:c656543830df 38 {
amateusz 3:c656543830df 39 this->set(0, value);
amateusz 3:c656543830df 40 this->set(1, value);
amateusz 3:c656543830df 41 };
amateusz 3:c656543830df 42 void YellowMotors::set(float value, int whichMotor )
amateusz 3:c656543830df 43 {
amateusz 3:c656543830df 44 pc.printf("value: %.3f", value);
amateusz 3:c656543830df 45 char directionsNew = _directions;
amateusz 3:c656543830df 46 if(whichMotor == -1) { // case set both;
amateusz 3:c656543830df 47 L = motorLinearizationL(abs(value));
amateusz 3:c656543830df 48 R = motorLinearizationL(abs(value));
amateusz 3:c656543830df 49 if(value > 0) {
amateusz 3:c656543830df 50 directionsNew &= ~L_B;
amateusz 3:c656543830df 51 directionsNew |= L_F;
amateusz 3:c656543830df 52 directionsNew &= ~R_B;
amateusz 3:c656543830df 53 directionsNew |= R_F;
amateusz 3:c656543830df 54 } else if (value < 0) {
amateusz 3:c656543830df 55 directionsNew &= ~L_F;
amateusz 3:c656543830df 56 directionsNew |= L_B;
amateusz 3:c656543830df 57 directionsNew &= ~R_F;
amateusz 3:c656543830df 58 directionsNew |= R_B;
amateusz 3:c656543830df 59 } else { // this and other " else value == 0 " update directions register to not allow any movement during reset/programming. Basiclly if the speed is to be 0, then turn the motors off.
amateusz 3:c656543830df 60 L = 0;
amateusz 3:c656543830df 61 R = 0;
amateusz 3:c656543830df 62 directionsNew |= L_B;
amateusz 3:c656543830df 63 directionsNew |= L_F;
amateusz 3:c656543830df 64 directionsNew |= R_B;
amateusz 3:c656543830df 65 directionsNew |= R_F;
amateusz 3:c656543830df 66 }
amateusz 3:c656543830df 67 } else {
amateusz 3:c656543830df 68 if(whichMotor == 0) {
amateusz 3:c656543830df 69 L = motorLinearizationL(abs(value));
amateusz 3:c656543830df 70 if(value > 0) {
amateusz 3:c656543830df 71 directionsNew &= ~L_B;
amateusz 3:c656543830df 72 directionsNew |= L_F;
amateusz 3:c656543830df 73 } else if (value < 0) {
amateusz 3:c656543830df 74 directionsNew &= ~L_F;
amateusz 3:c656543830df 75 directionsNew |= L_B;
amateusz 3:c656543830df 76 } else {
amateusz 3:c656543830df 77 L = 0;
amateusz 3:c656543830df 78 directionsNew |= L_B;
amateusz 3:c656543830df 79 directionsNew |= L_F;
amateusz 3:c656543830df 80 }
amateusz 3:c656543830df 81 } else if(whichMotor == 1) {
amateusz 3:c656543830df 82 R = motorLinearizationR(abs(value));
amateusz 3:c656543830df 83 if(value > 0) {
amateusz 3:c656543830df 84 directionsNew &= ~R_B;
amateusz 3:c656543830df 85 directionsNew |= R_F;
amateusz 3:c656543830df 86 } else if (value < 0) {
amateusz 3:c656543830df 87 directionsNew &= ~R_F;
amateusz 3:c656543830df 88 directionsNew |= R_B;
amateusz 3:c656543830df 89 } else {
amateusz 3:c656543830df 90 R = 0;
amateusz 3:c656543830df 91 directionsNew |= R_B;
amateusz 3:c656543830df 92 directionsNew |= R_F;
amateusz 3:c656543830df 93 }
amateusz 3:c656543830df 94 }
amateusz 3:c656543830df 95 }
amateusz 3:c656543830df 96 if ( _directions != directionsNew ) { // only change register content when needed.
amateusz 3:c656543830df 97 _directions = directionsNew;
amateusz 3:c656543830df 98 setDirections(_directions);
amateusz 3:c656543830df 99 }
amateusz 3:c656543830df 100 };
amateusz 3:c656543830df 101
amateusz 3:c656543830df 102 float YellowMotors::get(int whichMotor)
amateusz 3:c656543830df 103 {
amateusz 3:c656543830df 104 if(whichMotor == -1) { // case set both;
amateusz 3:c656543830df 105 return this->L._pwmSigned + this->R._pwmSigned;
amateusz 3:c656543830df 106 } else {
amateusz 3:c656543830df 107 if(whichMotor == 0) {
amateusz 3:c656543830df 108 return L._pwmSigned;
amateusz 3:c656543830df 109 } else if(whichMotor == 1) {
amateusz 3:c656543830df 110 return R._pwmSigned;
amateusz 3:c656543830df 111 }
amateusz 3:c656543830df 112 }
amateusz 3:c656543830df 113 return -1;
amateusz 3:c656543830df 114 };
amateusz 2:6f6e591f1838 115
amateusz 2:6f6e591f1838 116 void YellowMotors::setDirections(char directions)
amateusz 2:6f6e591f1838 117 {
amateusz 4:aef786c67b2d 118 if (_led != 0) *_led = 1;
amateusz 4:aef786c67b2d 119 pc.printf("\n\r_led ma adres: %d\n\r", _led);
amateusz 3:c656543830df 120 pc.printf("### set dirs: ");
amateusz 3:c656543830df 121 for(int i=0; i<8; ++i)
amateusz 3:c656543830df 122 pc.printf("%c", ((directions >> (7-i)) & 0x1)?'x':'_');
amateusz 3:c656543830df 123 pc.printf(" ###\n\r");
amateusz 2:6f6e591f1838 124 _directions = directions;
amateusz 2:6f6e591f1838 125 _lat = 0;
amateusz 2:6f6e591f1838 126 for (signed char i = 7; i >= 0; i--) {
amateusz 2:6f6e591f1838 127 _clk = 0;
amateusz 2:6f6e591f1838 128 _dat = (directions >> i) & 0x1;
amateusz 2:6f6e591f1838 129 wait(0.00001);
amateusz 2:6f6e591f1838 130 _clk = 1;
amateusz 2:6f6e591f1838 131 }
amateusz 2:6f6e591f1838 132 _lat = 1;
amateusz 2:6f6e591f1838 133 _clk = 0;
amateusz 4:aef786c67b2d 134 if (_led != 0) *_led = 0;
amateusz 2:6f6e591f1838 135 };
amateusz 2:6f6e591f1838 136
amateusz 4:aef786c67b2d 137 void YellowMotors::attachled(DigitalOut & led)
amateusz 4:aef786c67b2d 138 {
amateusz 4:aef786c67b2d 139 _led = &led;
amateusz 4:aef786c67b2d 140 }