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

FishMotors.cpp@8:dc13c3be6838, 2013-11-12 (annotated)

Committer:
calamaridudeman
Date:
Tue Nov 12 20:01:30 2013 +0000
Revision:
8:dc13c3be6838
Child:
20:97ab3d4c07b6
added my motor code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calamaridudeman 8:dc13c3be6838 1
calamaridudeman 8:dc13c3be6838 2 #include "mbed.h" // mbed library
calamaridudeman 8:dc13c3be6838 3 #include "QEI.h"
calamaridudeman 8:dc13c3be6838 4
calamaridudeman 8:dc13c3be6838 5 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
calamaridudeman 8:dc13c3be6838 6
calamaridudeman 8:dc13c3be6838 7 Ticker myTicker; // creates an instance of the ticker class, which can be used for running functions at a specified frequency.
calamaridudeman 8:dc13c3be6838 8 Ticker myTicker1;
calamaridudeman 8:dc13c3be6838 9
calamaridudeman 8:dc13c3be6838 10 FILE *fp;
calamaridudeman 8:dc13c3be6838 11
calamaridudeman 8:dc13c3be6838 12 // Declaring pins
calamaridudeman 8:dc13c3be6838 13 DigitalOut dA(p19); // pin 5 set as a digital output. Pins 5-30 can be used as digital outputs.
calamaridudeman 8:dc13c3be6838 14 DigitalOut dB(p20); // pin 6 set as digital input. Pins 5-30 can be used as digital inputs.
calamaridudeman 8:dc13c3be6838 15 PwmOut pwmOut1(p21); // pin 21 set as PWM output. Pins 21-26 can be used as PWM outputs.
calamaridudeman 8:dc13c3be6838 16 AnalogIn current(p15);
calamaridudeman 8:dc13c3be6838 17
calamaridudeman 8:dc13c3be6838 18 QEI motor1(p26, p25, NC, 1200, QEI::X4_ENCODING); // (encoder channel 1 pin, encoder channel 2 pin, index (n/a here), counts per revolution, mode (X4, X2))
calamaridudeman 8:dc13c3be6838 19 // Use line above verbatim to avoid difficulties. Only certain pins can be used for counting encoder ticks.
calamaridudeman 8:dc13c3be6838 20
calamaridudeman 8:dc13c3be6838 21 float speed = 0;
calamaridudeman 8:dc13c3be6838 22 float pos = 0;
calamaridudeman 8:dc13c3be6838 23 float timet = 0;
calamaridudeman 8:dc13c3be6838 24 float dCurrent1 = 0;
calamaridudeman 8:dc13c3be6838 25 float duty1 = 0;
calamaridudeman 8:dc13c3be6838 26 float mCurrent1 = 0;
calamaridudeman 8:dc13c3be6838 27 float dAngle1=0; // Desired Angle
calamaridudeman 8:dc13c3be6838 28 float dAngularVelocity1=0; // Desired Angular velocity
calamaridudeman 8:dc13c3be6838 29 float Angle1 = 0;
calamaridudeman 8:dc13c3be6838 30
calamaridudeman 8:dc13c3be6838 31
calamaridudeman 8:dc13c3be6838 32 float filterLowPass(float old, float current, float alpha){
calamaridudeman 8:dc13c3be6838 33 return (old+alpha*(current-old));
calamaridudeman 8:dc13c3be6838 34 }
calamaridudeman 8:dc13c3be6838 35
calamaridudeman 8:dc13c3be6838 36 void Control()
calamaridudeman 8:dc13c3be6838 37 {
calamaridudeman 8:dc13c3be6838 38 float preAngle1=Angle1; // Storing the angle in the previous time step
calamaridudeman 8:dc13c3be6838 39 Angle1 = motor1.getPulses()/1200.0*6.283; // Updating new motor angle
calamaridudeman 8:dc13c3be6838 40 speed =filterLowPass(speed,(Angle1-preAngle1)/.001, .2); // Updating new motor angluar vel
calamaridudeman 8:dc13c3be6838 41 mCurrent1 = current.read() *3.3/.14; //measure current
calamaridudeman 8:dc13c3be6838 42
calamaridudeman 8:dc13c3be6838 43 dCurrent1 = (3*(dAngle1-Angle1)+0.04*(dAngularVelocity1-speed));
calamaridudeman 8:dc13c3be6838 44
calamaridudeman 8:dc13c3be6838 45 if (dCurrent1>0){
calamaridudeman 8:dc13c3be6838 46 dA=1;
calamaridudeman 8:dc13c3be6838 47 dB=0;
calamaridudeman 8:dc13c3be6838 48 duty1= (abs(dCurrent1)*3.27+0.174*speed+10*(abs(dCurrent1)-mCurrent1))/12.0;
calamaridudeman 8:dc13c3be6838 49 //2.10- Resistance 0.2-Kv 5- current control gain, 12- source voltage
calamaridudeman 8:dc13c3be6838 50
calamaridudeman 8:dc13c3be6838 51 } else if (dCurrent1<0) {
calamaridudeman 8:dc13c3be6838 52 dA=0;
calamaridudeman 8:dc13c3be6838 53 dB=1;
calamaridudeman 8:dc13c3be6838 54 duty1= (abs(dCurrent1)*3.27-0.174*speed+10*(abs(dCurrent1)-mCurrent1))/12.0;}
calamaridudeman 8:dc13c3be6838 55
calamaridudeman 8:dc13c3be6838 56 if (duty1>0.99){
calamaridudeman 8:dc13c3be6838 57 duty1 =1;
calamaridudeman 8:dc13c3be6838 58 }
calamaridudeman 8:dc13c3be6838 59
calamaridudeman 8:dc13c3be6838 60 pwmOut1.write(duty1);
calamaridudeman 8:dc13c3be6838 61 }
calamaridudeman 8:dc13c3be6838 62
calamaridudeman 8:dc13c3be6838 63 void Printer(){
calamaridudeman 8:dc13c3be6838 64 fprintf(fp, "%f %f %f\n", duty1, mCurrent1, speed);
calamaridudeman 8:dc13c3be6838 65 }
calamaridudeman 8:dc13c3be6838 66
calamaridudeman 8:dc13c3be6838 67
calamaridudeman 8:dc13c3be6838 68 int main() {
calamaridudeman 8:dc13c3be6838 69 dCurrent1 = 3;
calamaridudeman 8:dc13c3be6838 70
calamaridudeman 8:dc13c3be6838 71 fp = fopen("/local/out.txt", "w"); // Open "out.txt" on the local file system for writing
calamaridudeman 8:dc13c3be6838 72 myTicker1.attach(&Printer, .005);
calamaridudeman 8:dc13c3be6838 73 myTicker.attach(&Control, .001);
calamaridudeman 8:dc13c3be6838 74
calamaridudeman 8:dc13c3be6838 75 pwmOut1.period_us(100);
calamaridudeman 8:dc13c3be6838 76
calamaridudeman 8:dc13c3be6838 77 //pwmOut.write(.5);
calamaridudeman 8:dc13c3be6838 78 wait(150);
calamaridudeman 8:dc13c3be6838 79 dCurrent1 = 0;
calamaridudeman 8:dc13c3be6838 80 myTicker.detach();
calamaridudeman 8:dc13c3be6838 81 myTicker1.detach();
calamaridudeman 8:dc13c3be6838 82 fclose(fp);
calamaridudeman 8:dc13c3be6838 83
calamaridudeman 8:dc13c3be6838 84 pwmOut1.write(0);
calamaridudeman 8:dc13c3be6838 85
calamaridudeman 8:dc13c3be6838 86 }