Carlo Collodi / kangaroo

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.

Dependencies:   QEI mbed

src/motor.cpp@25:8a34b8d6cc6e, 2013-11-19 (annotated)

Committer:
calamaridudeman
Date:
Tue Nov 19 20:42:01 2013 +0000
Revision:
25:8a34b8d6cc6e
Parent:
23:112c0be5a7f3
Child:
28:21773a6fb6df
modified master to start integrating motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calamaridudeman 23:112c0be5a7f3 1 #include "mbed.h"
calamaridudeman 23:112c0be5a7f3 2 #include "include/motor.hpp"
calamaridudeman 23:112c0be5a7f3 3
calamaridudeman 25:8a34b8d6cc6e 4 Motor::Motor(PinName aPin, PinName fPin, PinName bPin, PinName pwmPin, QEI enc):aIn(aPin),Forward(fPin),Backward(bPin),pwmOut(pwmPin),encoder(enc){
calamaridudeman 25:8a34b8d6cc6e 5 //15,17,18,21,qei25,26 and 16,19,20,22,qei23,24
calamaridudeman 23:112c0be5a7f3 6
calamaridudeman 23:112c0be5a7f3 7 freq =.001;
calamaridudeman 23:112c0be5a7f3 8 voltage = 12.0;
calamaridudeman 23:112c0be5a7f3 9 mode=0;
calamaridudeman 23:112c0be5a7f3 10
calamaridudeman 23:112c0be5a7f3 11 kp=3;
calamaridudeman 23:112c0be5a7f3 12 kd=0.04;
calamaridudeman 23:112c0be5a7f3 13
calamaridudeman 23:112c0be5a7f3 14 pwmOut.period_us(.0005);
calamaridudeman 23:112c0be5a7f3 15 }
calamaridudeman 23:112c0be5a7f3 16
calamaridudeman 23:112c0be5a7f3 17 void Motor::start(){
calamaridudeman 23:112c0be5a7f3 18 t.attach(this, &Motor::Control, freq);
calamaridudeman 23:112c0be5a7f3 19 }
calamaridudeman 23:112c0be5a7f3 20
calamaridudeman 23:112c0be5a7f3 21 void Motor::stop(){
calamaridudeman 23:112c0be5a7f3 22 t.detach();
calamaridudeman 23:112c0be5a7f3 23 pwmOut.write(0);
calamaridudeman 25:8a34b8d6cc6e 24 dAngle=0;
calamaridudeman 25:8a34b8d6cc6e 25 dAngularVelocity=0;
calamaridudeman 23:112c0be5a7f3 26 }
calamaridudeman 23:112c0be5a7f3 27
calamaridudeman 23:112c0be5a7f3 28 void Motor::zero(){
calamaridudeman 23:112c0be5a7f3 29 encoder.reset();
calamaridudeman 23:112c0be5a7f3 30 }
calamaridudeman 23:112c0be5a7f3 31
calamaridudeman 23:112c0be5a7f3 32 void Motor::Control(){
calamaridudeman 23:112c0be5a7f3 33 float preAngle=angle; // Storing the angle in the previous time step
calamaridudeman 23:112c0be5a7f3 34 angle = encoder.getPulses()/1200.0*6.283; // Updating new motor angle
calamaridudeman 23:112c0be5a7f3 35 speed =Motor::filterLowPass(speed,(angle-preAngle)/freq, .2); // Updating new motor angular vel
calamaridudeman 23:112c0be5a7f3 36 float mCurrent = getCurrent();
calamaridudeman 23:112c0be5a7f3 37
calamaridudeman 23:112c0be5a7f3 38 float dCurrent;
calamaridudeman 23:112c0be5a7f3 39 switch(mode){
calamaridudeman 23:112c0be5a7f3 40 case 1:
calamaridudeman 23:112c0be5a7f3 41 dCurrent = kp*(dAngle-angle);
calamaridudeman 23:112c0be5a7f3 42 case 2:
calamaridudeman 23:112c0be5a7f3 43 dCurrent = kd*(dAngularVelocity-speed);
calamaridudeman 23:112c0be5a7f3 44 case 3:
calamaridudeman 23:112c0be5a7f3 45 dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
calamaridudeman 23:112c0be5a7f3 46 case 0:
calamaridudeman 23:112c0be5a7f3 47 dCurrent = dTorque/3.27;
calamaridudeman 23:112c0be5a7f3 48 }
calamaridudeman 23:112c0be5a7f3 49
calamaridudeman 23:112c0be5a7f3 50 float duty=0;
calamaridudeman 23:112c0be5a7f3 51 if (dCurrent>0){
calamaridudeman 23:112c0be5a7f3 52 Forward=1;
calamaridudeman 23:112c0be5a7f3 53 Backward=0;
calamaridudeman 23:112c0be5a7f3 54 duty= (abs(dCurrent)*3.27+0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
calamaridudeman 23:112c0be5a7f3 55 }else if (dCurrent<0){
calamaridudeman 23:112c0be5a7f3 56 Forward=0;
calamaridudeman 23:112c0be5a7f3 57 Backward=1;
calamaridudeman 23:112c0be5a7f3 58 duty= (abs(dCurrent)*3.27-0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
calamaridudeman 23:112c0be5a7f3 59 }
calamaridudeman 23:112c0be5a7f3 60
calamaridudeman 23:112c0be5a7f3 61 if (duty>0.99){
calamaridudeman 23:112c0be5a7f3 62 duty =1;
calamaridudeman 23:112c0be5a7f3 63 }
calamaridudeman 23:112c0be5a7f3 64
calamaridudeman 23:112c0be5a7f3 65 pwmOut.write(duty);
calamaridudeman 23:112c0be5a7f3 66 }
calamaridudeman 23:112c0be5a7f3 67
calamaridudeman 23:112c0be5a7f3 68 float Motor::getCurrent()
calamaridudeman 23:112c0be5a7f3 69 {
calamaridudeman 23:112c0be5a7f3 70 return (1/.140)*3.3 * aIn;
calamaridudeman 23:112c0be5a7f3 71 }
calamaridudeman 23:112c0be5a7f3 72
calamaridudeman 23:112c0be5a7f3 73 float Motor::getMotorPos()
calamaridudeman 23:112c0be5a7f3 74 {
calamaridudeman 23:112c0be5a7f3 75 return encoder.getPulses();
calamaridudeman 23:112c0be5a7f3 76 }
calamaridudeman 23:112c0be5a7f3 77
calamaridudeman 23:112c0be5a7f3 78 float Motor::filterLowPass(float old, float currentIn, float alphar){
calamaridudeman 23:112c0be5a7f3 79 return (old+alphar*(currentIn-old));
calamaridudeman 23:112c0be5a7f3 80 }
calamaridudeman 23:112c0be5a7f3 81
calamaridudeman 23:112c0be5a7f3 82 void Motor::setTorque(float t){
calamaridudeman 23:112c0be5a7f3 83 mode = 0;
calamaridudeman 23:112c0be5a7f3 84 dTorque = t;
calamaridudeman 23:112c0be5a7f3 85 }
calamaridudeman 23:112c0be5a7f3 86
calamaridudeman 23:112c0be5a7f3 87 void Motor::setPos(float pos){
calamaridudeman 23:112c0be5a7f3 88 mode = 1;
calamaridudeman 23:112c0be5a7f3 89 dAngle=pos;
calamaridudeman 23:112c0be5a7f3 90 }
calamaridudeman 23:112c0be5a7f3 91
calamaridudeman 23:112c0be5a7f3 92 void Motor::setVel(float vel){
calamaridudeman 23:112c0be5a7f3 93 mode = 2;
calamaridudeman 23:112c0be5a7f3 94 dAngularVelocity = vel;
calamaridudeman 23:112c0be5a7f3 95 }
calamaridudeman 23:112c0be5a7f3 96
calamaridudeman 23:112c0be5a7f3 97 void Motor::setPosVel(float pos, float vel){
calamaridudeman 23:112c0be5a7f3 98 mode = 3;
calamaridudeman 23:112c0be5a7f3 99 dAngle = pos;
calamaridudeman 23:112c0be5a7f3 100 dAngularVelocity = vel;
calamaridudeman 23:112c0be5a7f3 101 }