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@3:c656543830df, 2016-02-04 (annotated)

Committer:
amateusz
Date:
Thu Feb 04 15:45:09 2016 +0000
Revision:
3:c656543830df
Parent:
2:6f6e591f1838
Child:
4:aef786c67b2d
So now there is a class representing a shield, which allows for quite convenient control of motors' speed and direction. Nested sub class for motors themselfs is prepared to be extended with.. speed sensor! Some day.

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 2:6f6e591f1838 8 return (float)((6.6691*(exp(0.0249053*(desired*100))+2.92508))/100.0);
amateusz 2:6f6e591f1838 9 };
amateusz 0:85e85976c650 10
amateusz 0:85e85976c650 11 float motorLinearizationR(float desired)
amateusz 0:85e85976c650 12 {
amateusz 2:6f6e591f1838 13 return (float)(4.096509*(exp(0.0286952296*(desired*100))+5.073644964)/100.0); // \/ 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 3:c656543830df 20 _directions = 0xff;
amateusz 2:6f6e591f1838 21 //PwmOut _Lpwm(D11);
amateusz 2:6f6e591f1838 22 //PwmOut _Rpwm(D3);
amateusz 2:6f6e591f1838 23 //DigitalOut _dat(dat);
amateusz 2:6f6e591f1838 24 //DigitalOut _clk(clk);
amateusz 2:6f6e591f1838 25 //DigitalOut _lat(lat);
amateusz 2:6f6e591f1838 26
amateusz 3:c656543830df 27 //motorItself::motorItself L(Lpwm);
amateusz 3:c656543830df 28 //motorItself::motorItself R(Rpwm);
amateusz 2:6f6e591f1838 29 // and create motor objects
amateusz 2:6f6e591f1838 30 _ena.write(0);
amateusz 2:6f6e591f1838 31 //pc.printf("konstruktor yellow. Lpwm: %d\n\r", Lpwm);
amateusz 3:c656543830df 32
amateusz 2:6f6e591f1838 33 }
amateusz 3:c656543830df 34 void YellowMotors::operator= (const float value)
amateusz 3:c656543830df 35 {
amateusz 3:c656543830df 36 this->set(0, value);
amateusz 3:c656543830df 37 this->set(1, value);
amateusz 3:c656543830df 38 };
amateusz 3:c656543830df 39 void YellowMotors::set(float value, int whichMotor )
amateusz 3:c656543830df 40 {
amateusz 3:c656543830df 41 pc.printf("value: %.3f", value);
amateusz 3:c656543830df 42 char directionsNew = _directions;
amateusz 3:c656543830df 43 if(whichMotor == -1) { // case set both;
amateusz 3:c656543830df 44 L = motorLinearizationL(abs(value));
amateusz 3:c656543830df 45 R = motorLinearizationL(abs(value));
amateusz 3:c656543830df 46 if(value > 0) {
amateusz 3:c656543830df 47 directionsNew &= ~L_B;
amateusz 3:c656543830df 48 directionsNew |= L_F;
amateusz 3:c656543830df 49 directionsNew &= ~R_B;
amateusz 3:c656543830df 50 directionsNew |= R_F;
amateusz 3:c656543830df 51 } else if (value < 0) {
amateusz 3:c656543830df 52 directionsNew &= ~L_F;
amateusz 3:c656543830df 53 directionsNew |= L_B;
amateusz 3:c656543830df 54 directionsNew &= ~R_F;
amateusz 3:c656543830df 55 directionsNew |= R_B;
amateusz 3:c656543830df 56 } 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 57 L = 0;
amateusz 3:c656543830df 58 R = 0;
amateusz 3:c656543830df 59 directionsNew |= L_B;
amateusz 3:c656543830df 60 directionsNew |= L_F;
amateusz 3:c656543830df 61 directionsNew |= R_B;
amateusz 3:c656543830df 62 directionsNew |= R_F;
amateusz 3:c656543830df 63 }
amateusz 3:c656543830df 64 } else {
amateusz 3:c656543830df 65 if(whichMotor == 0) {
amateusz 3:c656543830df 66 L = motorLinearizationL(abs(value));
amateusz 3:c656543830df 67 if(value > 0) {
amateusz 3:c656543830df 68 directionsNew &= ~L_B;
amateusz 3:c656543830df 69 directionsNew |= L_F;
amateusz 3:c656543830df 70 } else if (value < 0) {
amateusz 3:c656543830df 71 directionsNew &= ~L_F;
amateusz 3:c656543830df 72 directionsNew |= L_B;
amateusz 3:c656543830df 73 } else {
amateusz 3:c656543830df 74 L = 0;
amateusz 3:c656543830df 75 directionsNew |= L_B;
amateusz 3:c656543830df 76 directionsNew |= L_F;
amateusz 3:c656543830df 77 }
amateusz 3:c656543830df 78 } else if(whichMotor == 1) {
amateusz 3:c656543830df 79 R = motorLinearizationR(abs(value));
amateusz 3:c656543830df 80 if(value > 0) {
amateusz 3:c656543830df 81 directionsNew &= ~R_B;
amateusz 3:c656543830df 82 directionsNew |= R_F;
amateusz 3:c656543830df 83 } else if (value < 0) {
amateusz 3:c656543830df 84 directionsNew &= ~R_F;
amateusz 3:c656543830df 85 directionsNew |= R_B;
amateusz 3:c656543830df 86 } else {
amateusz 3:c656543830df 87 R = 0;
amateusz 3:c656543830df 88 directionsNew |= R_B;
amateusz 3:c656543830df 89 directionsNew |= R_F;
amateusz 3:c656543830df 90 }
amateusz 3:c656543830df 91 }
amateusz 3:c656543830df 92 }
amateusz 3:c656543830df 93 if ( _directions != directionsNew ) { // only change register content when needed.
amateusz 3:c656543830df 94 _directions = directionsNew;
amateusz 3:c656543830df 95 setDirections(_directions);
amateusz 3:c656543830df 96 }
amateusz 3:c656543830df 97 };
amateusz 3:c656543830df 98
amateusz 3:c656543830df 99 float YellowMotors::get(int whichMotor)
amateusz 3:c656543830df 100 {
amateusz 3:c656543830df 101 if(whichMotor == -1) { // case set both;
amateusz 3:c656543830df 102 return this->L._pwmSigned + this->R._pwmSigned;
amateusz 3:c656543830df 103 } else {
amateusz 3:c656543830df 104 if(whichMotor == 0) {
amateusz 3:c656543830df 105 return L._pwmSigned;
amateusz 3:c656543830df 106 } else if(whichMotor == 1) {
amateusz 3:c656543830df 107 return R._pwmSigned;
amateusz 3:c656543830df 108 }
amateusz 3:c656543830df 109 }
amateusz 3:c656543830df 110 return -1;
amateusz 3:c656543830df 111 };
amateusz 2:6f6e591f1838 112
amateusz 2:6f6e591f1838 113 void YellowMotors::setDirections(char directions)
amateusz 2:6f6e591f1838 114 {
amateusz 3:c656543830df 115 pc.printf("### set dirs: ");
amateusz 3:c656543830df 116 for(int i=0; i<8; ++i)
amateusz 3:c656543830df 117 pc.printf("%c", ((directions >> (7-i)) & 0x1)?'x':'_');
amateusz 3:c656543830df 118 pc.printf(" ###\n\r");
amateusz 2:6f6e591f1838 119 _directions = directions;
amateusz 2:6f6e591f1838 120 _lat = 0;
amateusz 2:6f6e591f1838 121 for (signed char i = 7; i >= 0; i--) {
amateusz 2:6f6e591f1838 122 _clk = 0;
amateusz 2:6f6e591f1838 123 _dat = (directions >> i) & 0x1;
amateusz 2:6f6e591f1838 124 wait(0.00001);
amateusz 2:6f6e591f1838 125 _clk = 1;
amateusz 2:6f6e591f1838 126 }
amateusz 2:6f6e591f1838 127 _lat = 1;
amateusz 2:6f6e591f1838 128 _clk = 0;
amateusz 2:6f6e591f1838 129 };
amateusz 2:6f6e591f1838 130