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@53:978b7fa74080, 2013-12-02 (annotated)

Committer:
calamaridudeman
Date:
Mon Dec 02 05:16:26 2013 +0000
Revision:
53:978b7fa74080
Parent:
50:510ce69c2a10 
shin works too, now to get event detection

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 37:bf257a0154db 4 Motor::Motor(PinName aPin, PinName fPin, PinName bPin, PinName pwmPin, QEI &enc):aIn(aPin),Forward(fPin),Backward(bPin),pwmOut(pwmPin),encoder(enc){
calamaridudeman 37:bf257a0154db 5 //15,17,18,21,qei25,26 and 16,19,20,22,qei23,24
calamaridudeman 43:68faf056ed5c 6 freq =.0005;
calamaridudeman 43:68faf056ed5c 7 speed=0;
calamaridudeman 23:112c0be5a7f3 8 voltage = 12.0;
calamaridudeman 23:112c0be5a7f3 9 mode=0;
calamaridudeman 23:112c0be5a7f3 10
calamaridudeman 44:3566c5699ba6 11 pos=0;
calamaridudeman 44:3566c5699ba6 12
calamaridudeman 53:978b7fa74080 13 kp=4;
calamaridudeman 23:112c0be5a7f3 14 kd=0.04;
calamaridudeman 23:112c0be5a7f3 15
calamaridudeman 53:978b7fa74080 16 dAngle=0;
calamaridudeman 53:978b7fa74080 17
calamaridudeman 43:68faf056ed5c 18 pwmOut.period_us(100);
calamaridudeman 23:112c0be5a7f3 19 }
calamaridudeman 23:112c0be5a7f3 20
calamaridudeman 23:112c0be5a7f3 21 void Motor::start(){
calamaridudeman 23:112c0be5a7f3 22 t.attach(this, &Motor::Control, freq);
calamaridudeman 23:112c0be5a7f3 23 }
calamaridudeman 23:112c0be5a7f3 24
sherryxy 49:3aaa790800ad 25 void Motor::calibAngle(float ang){
calamaridudeman 53:978b7fa74080 26 encoder.calibPulse((int)(ang*1200/6.283));
sherryxy 50:510ce69c2a10 27 dAngle=ang;
sherryxy 49:3aaa790800ad 28 }
sherryxy 49:3aaa790800ad 29
calamaridudeman 23:112c0be5a7f3 30 void Motor::stop(){
calamaridudeman 23:112c0be5a7f3 31 t.detach();
calamaridudeman 23:112c0be5a7f3 32 pwmOut.write(0);
calamaridudeman 37:bf257a0154db 33 dAngle=0;
calamaridudeman 37:bf257a0154db 34 dAngularVelocity=0;
calamaridudeman 23:112c0be5a7f3 35 }
calamaridudeman 23:112c0be5a7f3 36
calamaridudeman 23:112c0be5a7f3 37 void Motor::zero(){
calamaridudeman 23:112c0be5a7f3 38 encoder.reset();
calamaridudeman 23:112c0be5a7f3 39 }
calamaridudeman 23:112c0be5a7f3 40
sherryxy 49:3aaa790800ad 41 float Motor::getAngle(){
sherryxy 49:3aaa790800ad 42 return encoder.getPulses()/1200.0*6.283;
sherryxy 49:3aaa790800ad 43 }
sherryxy 49:3aaa790800ad 44
calamaridudeman 23:112c0be5a7f3 45 void Motor::Control(){
calamaridudeman 23:112c0be5a7f3 46 float preAngle=angle; // Storing the angle in the previous time step
calamaridudeman 23:112c0be5a7f3 47 angle = encoder.getPulses()/1200.0*6.283; // Updating new motor angle
calamaridudeman 23:112c0be5a7f3 48 speed =Motor::filterLowPass(speed,(angle-preAngle)/freq, .2); // Updating new motor angular vel
calamaridudeman 23:112c0be5a7f3 49 float mCurrent = getCurrent();
calamaridudeman 43:68faf056ed5c 50 //pc.printf("current: %f speed: %f\n", mCurrent, speed);
calamaridudeman 23:112c0be5a7f3 51
calamaridudeman 44:3566c5699ba6 52 float dCurrent=0;
calamaridudeman 23:112c0be5a7f3 53 switch(mode){
alexc89 45:0db0fc9f77b1 54 default:{
calamaridudeman 44:3566c5699ba6 55 //dCurrent = kp*(dAngle-angle)+kd*(dAngularVelocity-speed);
calamaridudeman 44:3566c5699ba6 56 dCurrent = dTorque/3.27;
calamaridudeman 44:3566c5699ba6 57 break;
calamaridudeman 44:3566c5699ba6 58 }
alexc89 45:0db0fc9f77b1 59 case 1:{//Positional Control
calamaridudeman 23:112c0be5a7f3 60 dCurrent = kp*(dAngle-angle);
calamaridudeman 44:3566c5699ba6 61 break;
calamaridudeman 44:3566c5699ba6 62 }
alexc89 45:0db0fc9f77b1 63 case 2:{//Velocity Control
alexc89 45:0db0fc9f77b1 64 dCurrent = kp*(dAngularVelocity-speed);
calamaridudeman 44:3566c5699ba6 65 break;
calamaridudeman 44:3566c5699ba6 66 }
alexc89 45:0db0fc9f77b1 67 case 3:{ //Torque Control
calamaridudeman 43:68faf056ed5c 68 dCurrent = dTorque/3.27;
calamaridudeman 44:3566c5699ba6 69 break;
calamaridudeman 44:3566c5699ba6 70 }
calamaridudeman 23:112c0be5a7f3 71 }
calamaridudeman 43:68faf056ed5c 72
calamaridudeman 23:112c0be5a7f3 73
calamaridudeman 23:112c0be5a7f3 74 float duty=0;
calamaridudeman 23:112c0be5a7f3 75 if (dCurrent>0){
calamaridudeman 23:112c0be5a7f3 76 Forward=1;
calamaridudeman 23:112c0be5a7f3 77 Backward=0;
calamaridudeman 23:112c0be5a7f3 78 duty= (abs(dCurrent)*3.27+0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
calamaridudeman 23:112c0be5a7f3 79 }else if (dCurrent<0){
calamaridudeman 23:112c0be5a7f3 80 Forward=0;
calamaridudeman 23:112c0be5a7f3 81 Backward=1;
calamaridudeman 23:112c0be5a7f3 82 duty= (abs(dCurrent)*3.27-0.174*speed+10*(abs(dCurrent)-mCurrent))/voltage;
calamaridudeman 23:112c0be5a7f3 83 }
calamaridudeman 23:112c0be5a7f3 84
calamaridudeman 23:112c0be5a7f3 85 if (duty>0.99){
calamaridudeman 23:112c0be5a7f3 86 duty =1;
calamaridudeman 23:112c0be5a7f3 87 }
calamaridudeman 43:68faf056ed5c 88 pwmOut.period_us(100);
calamaridudeman 43:68faf056ed5c 89 pwmOut.write(duty);
calamaridudeman 23:112c0be5a7f3 90 }
calamaridudeman 23:112c0be5a7f3 91
calamaridudeman 23:112c0be5a7f3 92 float Motor::getCurrent()
calamaridudeman 23:112c0be5a7f3 93 {
calamaridudeman 43:68faf056ed5c 94 return (1/.140)*3.3 * aIn.read();
calamaridudeman 23:112c0be5a7f3 95 }
calamaridudeman 23:112c0be5a7f3 96
calamaridudeman 37:bf257a0154db 97 int Motor::getPos()
calamaridudeman 23:112c0be5a7f3 98 {
calamaridudeman 23:112c0be5a7f3 99 return encoder.getPulses();
calamaridudeman 23:112c0be5a7f3 100 }
calamaridudeman 23:112c0be5a7f3 101
calamaridudeman 23:112c0be5a7f3 102 float Motor::filterLowPass(float old, float currentIn, float alphar){
calamaridudeman 23:112c0be5a7f3 103 return (old+alphar*(currentIn-old));
calamaridudeman 23:112c0be5a7f3 104 }
calamaridudeman 23:112c0be5a7f3 105
calamaridudeman 23:112c0be5a7f3 106 void Motor::setTorque(float t){
calamaridudeman 43:68faf056ed5c 107 mode = 3;
calamaridudeman 23:112c0be5a7f3 108 dTorque = t;
calamaridudeman 23:112c0be5a7f3 109 }
calamaridudeman 23:112c0be5a7f3 110
calamaridudeman 23:112c0be5a7f3 111 void Motor::setPos(float pos){
alexc89 45:0db0fc9f77b1 112 mode = 1;
calamaridudeman 23:112c0be5a7f3 113 dAngle=pos;
calamaridudeman 43:68faf056ed5c 114 dAngularVelocity = 0;
calamaridudeman 23:112c0be5a7f3 115 }
calamaridudeman 23:112c0be5a7f3 116
calamaridudeman 23:112c0be5a7f3 117 void Motor::setVel(float vel){
calamaridudeman 23:112c0be5a7f3 118 mode = 2;
calamaridudeman 23:112c0be5a7f3 119 dAngularVelocity = vel;
calamaridudeman 23:112c0be5a7f3 120 }
calamaridudeman 23:112c0be5a7f3 121
calamaridudeman 23:112c0be5a7f3 122 void Motor::setPosVel(float pos, float vel){
calamaridudeman 43:68faf056ed5c 123 mode = -1;
calamaridudeman 23:112c0be5a7f3 124 dAngle = pos;
calamaridudeman 23:112c0be5a7f3 125 dAngularVelocity = vel;
calamaridudeman 23:112c0be5a7f3 126 }