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@29:33c5c61f1068, 2013-11-24 (annotated)

Committer:
calamaridudeman
Date:
Sun Nov 24 19:21:47 2013 +0000
Revision:
29:33c5c61f1068
Parent:
28:21773a6fb6df 
Added & everywhere (including fields!) to ensure no copying of encoders (which breaks the interrupts).  All motor encoders reporting properly now

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