Peihsun Yeh
/
ECE4180FinalV4
CarlsenBot working
Diff: Motor/Motor.cpp
- Revision:
- 0:cdd566529530
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor/Motor.cpp Wed May 01 16:00:53 2013 +0000 @@ -0,0 +1,141 @@ +/* mbed simple H-bridge motor controller + * Copyright (c) 2007-2010, sford, http://mbed.org + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include "Motor.h" + +#include "mbed.h" +DigitalOut led1(LED1); +int count = 0; + +Motor::Motor(PinName pwm, PinName fwd, PinName rev): + _pwm(pwm), _fwd(fwd), _rev(rev) { + + // Set initial condition of PWM + _pwm.period(0.001); + _pwm = 0; + + // Initial condition of output enables + _fwd = 0; + _rev = 0; +} + +// Motor with i2c encoder +Motor::Motor(PinName pwm, PinName fwd, PinName rev, I2C *i): + _pwm(pwm), _fwd(fwd), _rev(rev), _i2c(i) { + + // Set initial condition of PWM + _pwm.period(0.001); + _pwm = 0; + + // Initial condition of output enables + _fwd = 0; + _rev = 0; + + ticks = 0; + is_reversed = false; + deviceAddress = 0x60; +} + +void Motor::speed(float speed) { + _fwd = (speed > 0.0); + _rev = (speed < 0.0); + _pwm = abs(speed); +} + +void Motor::init(char address, int ID, bool term){ + char toWrite[] = {0x4D, address}; // writing to 0x4D tells encoder that you want to change its address + _i2c->write(0x60, toWrite, 2); + wait(0.01); + // encoders are daisy-chained i2c devices. Writing to 0x4B tells the device you mean to put another after it + if(!term){ + char disableTerm = 0x4B; + _i2c->write(address, &disableTerm, 1); + wait(0.01); + } + deviceAddress = address; + motorID = ID; + resetTicks(); + wait(0.01); +} + +void Motor::move(int pos){ + float diff1, spd1, lower = 0.09, upper = 0.1, integral1 = 0.0; + //int K = 2612, Ki = 0.001; + int K = 6500, Ki = 0.001; + while (!(abs(float(ticks - pos)) <= 20)) + { + diff1 = (pos - ticks); + integral1 += (diff1*0.001); + spd1 = (diff1/K) + (Ki*integral1); + //rationalize a speed + if (spd1 >= upper) spd1 = upper; + else if(spd1 <= lower && spd1 > 0.0) spd1 = lower; + else if(spd1 < 0.0 && spd1 >= -lower) spd1 = -lower; + else if(spd1 <= -upper) spd1 = -upper; + //printf("Motor %d is at position %d, moving with speed %f\n", motorID, ticks, spd1); // debugging + speed(spd1); + wait(0.001); + + }//while + speed(0); + wait(0.5); + // Adjusting + while (!(abs(float(ticks - pos)) <= 10)) + { + diff1 = (pos - ticks); + spd1 = 0.04; + //printf("Motor %d is at position %d, moving with speed %f\n", motorID, ticks, spd1); // debugging + //rationalize a speed + if(diff1 < 0) spd1 = -spd1; + speed(spd1); + wait(0.001); + + }//while + speed(0); + wait(0.8); + //printf("Done! Motor %d is at position %d\n", motorID, ticks); // debugging +} + +void Motor::getTicks(){ + char lsb, msb; + char addr[2] = {0x40, 0x41}; // look on datasheet for this + _i2c->write(deviceAddress, &addr[0], 1); + _i2c->read(deviceAddress, &msb, 1); + _i2c->write(deviceAddress, &addr[1], 1); + _i2c->read(deviceAddress, &lsb, 1); + ticks = (msb << 8) | lsb; + if(is_reversed){ + if(ticks != 0) ticks = 65536-ticks; + else ticks = 0; + } + if(ticks > 50000){ // When moving towards 0, you may overshoot and fill the buffer, and it will count backwards from 2^16 - 1. + ticks = 0; // 50000 is rather arbitrary, it just lets me know I've gone backwards from 0. This isn't a great fix. + } +} + +void Motor::resetTicks(){ + char reset = 0x4A; + _i2c->write(deviceAddress, &reset, 1); +} + + +