Motor.cpp

00001 /* mbed simple H-bridge motor controller
00002  * Copyright (c) 2007-2010, sford, http://mbed.org
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a copy
00005  * of this software and associated documentation files (the "Software"), to deal
00006  * in the Software without restriction, including without limitation the rights
00007  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008  * copies of the Software, and to permit persons to whom the Software is
00009  * furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included in
00012  * all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020  * THE SOFTWARE.
00021  */
00022 
00023 #include "Motor.h"
00024 
00025 #include "mbed.h"
00026 DigitalOut led1(LED1);
00027 int count = 0;
00028 
00029 Motor::Motor(PinName pwm, PinName fwd, PinName rev):
00030         _pwm(pwm), _fwd(fwd), _rev(rev) {
00031 
00032     // Set initial condition of PWM
00033     _pwm.period(0.001);
00034     _pwm = 0;
00035 
00036     // Initial condition of output enables
00037     _fwd = 0;
00038     _rev = 0;    
00039 }
00040 
00041 // Motor with i2c encoder
00042 Motor::Motor(PinName pwm, PinName fwd, PinName rev, I2C *i):
00043         _pwm(pwm), _fwd(fwd), _rev(rev), _i2c(i) {
00044 
00045     // Set initial condition of PWM
00046     _pwm.period(0.001);
00047     _pwm = 0;
00048 
00049     // Initial condition of output enables
00050     _fwd = 0;
00051     _rev = 0;
00052     
00053     ticks = 0;
00054     is_reversed = false;
00055     deviceAddress = 0x60;
00056 }
00057 
00058 void Motor::speed(float speed) {
00059     _fwd = (speed > 0.0);
00060     _rev = (speed < 0.0);
00061     _pwm = abs(speed);
00062 }
00063 
00064 void Motor::init(char address, int ID, bool term){
00065     char toWrite[] = {0x4D, address}; // writing to 0x4D tells encoder that you want to change its address
00066     _i2c->write(0x60, toWrite, 2); 
00067     wait(0.01);
00068     // encoders are daisy-chained i2c devices. Writing to 0x4B tells the device you mean to put another after it
00069     if(!term){ 
00070         char disableTerm = 0x4B;
00071         _i2c->write(address, &disableTerm, 1);
00072         wait(0.01);
00073     }
00074     deviceAddress = address;
00075     motorID = ID;
00076     resetTicks();
00077     wait(0.01);
00078 }
00079 
00080 void Motor::move(int pos){
00081     float diff1, spd1, lower = 0.09, upper = 0.1, integral1 = 0.0;
00082     //int K = 2612, Ki = 0.001;
00083     int K = 6500, Ki = 0.001;
00084     while (!(abs(float(ticks - pos)) <= 20))
00085     {    
00086         diff1 = (pos - ticks);
00087         integral1 += (diff1*0.001);
00088         spd1 = (diff1/K) + (Ki*integral1);      
00089         //rationalize a speed 
00090         if (spd1 >= upper) spd1 = upper;
00091         else if(spd1 <= lower && spd1 > 0.0) spd1 = lower;
00092         else if(spd1 < 0.0 && spd1 >= -lower) spd1 = -lower;
00093         else if(spd1 <= -upper) spd1 = -upper;  
00094         //printf("Motor %d is at position %d, moving with speed %f\n", motorID, ticks, spd1); // debugging
00095         speed(spd1);
00096         wait(0.001);
00097         
00098     }//while 
00099     speed(0);
00100     wait(0.5);
00101     // Adjusting
00102     while (!(abs(float(ticks - pos)) <= 10))
00103     {    
00104         diff1 = (pos - ticks);
00105         spd1 = 0.04;       
00106         //printf("Motor %d is at position %d, moving with speed %f\n", motorID, ticks, spd1); // debugging
00107         //rationalize a speed 
00108         if(diff1 < 0) spd1 = -spd1;
00109         speed(spd1);
00110         wait(0.001);
00111         
00112     }//while 
00113     speed(0);
00114     wait(0.8);
00115     //printf("Done! Motor %d is at position %d\n", motorID, ticks); // debugging
00116 }
00117 
00118 void Motor::getTicks(){
00119     char lsb, msb; 
00120     char addr[2] = {0x40, 0x41}; // look on datasheet for this
00121     _i2c->write(deviceAddress, &addr[0], 1);
00122     _i2c->read(deviceAddress, &msb, 1);
00123     _i2c->write(deviceAddress, &addr[1], 1);
00124     _i2c->read(deviceAddress, &lsb, 1);
00125     ticks = (msb << 8) | lsb;
00126     if(is_reversed){
00127         if(ticks != 0) ticks = 65536-ticks;
00128         else ticks = 0;
00129     }
00130     if(ticks > 50000){ // When moving towards 0, you may overshoot and fill the buffer, and it will count backwards from 2^16 - 1. 
00131         ticks = 0;     // 50000 is rather arbitrary, it just lets me know I've gone backwards from 0. This isn't a great fix. 
00132     }
00133 }
00134 
00135 void Motor::resetTicks(){
00136     char reset = 0x4A;
00137     _i2c->write(deviceAddress, &reset, 1);
00138 }
00139 
00140 
00141