Peihsun Yeh
/
ECE4180FinalV4
CarlsenBot working
Motor/Motor.cpp
- Committer:
- pyeh9
- Date:
- 2013-05-01
- Revision:
- 0:cdd566529530
File content as of revision 0:cdd566529530:
/* 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); }