Peihsun Yeh / Mbed 2 deprecated ECE4180FinalV4

CarlsenBot working

Dependencies:   Servo mbed

Motor/Motor.cpp@0:cdd566529530, 2013-05-01 (annotated)

Committer:
pyeh9
Date:
Wed May 01 16:00:53 2013 +0000
Revision:
0:cdd566529530
CarlsenBot Working commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyeh9 0:cdd566529530 1 /* mbed simple H-bridge motor controller
pyeh9 0:cdd566529530 2 * Copyright (c) 2007-2010, sford, http://mbed.org
pyeh9 0:cdd566529530 3 *
pyeh9 0:cdd566529530 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
pyeh9 0:cdd566529530 5 * of this software and associated documentation files (the "Software"), to deal
pyeh9 0:cdd566529530 6 * in the Software without restriction, including without limitation the rights
pyeh9 0:cdd566529530 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
pyeh9 0:cdd566529530 8 * copies of the Software, and to permit persons to whom the Software is
pyeh9 0:cdd566529530 9 * furnished to do so, subject to the following conditions:
pyeh9 0:cdd566529530 10 *
pyeh9 0:cdd566529530 11 * The above copyright notice and this permission notice shall be included in
pyeh9 0:cdd566529530 12 * all copies or substantial portions of the Software.
pyeh9 0:cdd566529530 13 *
pyeh9 0:cdd566529530 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
pyeh9 0:cdd566529530 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
pyeh9 0:cdd566529530 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
pyeh9 0:cdd566529530 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
pyeh9 0:cdd566529530 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
pyeh9 0:cdd566529530 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
pyeh9 0:cdd566529530 20 * THE SOFTWARE.
pyeh9 0:cdd566529530 21 */
pyeh9 0:cdd566529530 22
pyeh9 0:cdd566529530 23 #include "Motor.h"
pyeh9 0:cdd566529530 24
pyeh9 0:cdd566529530 25 #include "mbed.h"
pyeh9 0:cdd566529530 26 DigitalOut led1(LED1);
pyeh9 0:cdd566529530 27 int count = 0;
pyeh9 0:cdd566529530 28
pyeh9 0:cdd566529530 29 Motor::Motor(PinName pwm, PinName fwd, PinName rev):
pyeh9 0:cdd566529530 30 _pwm(pwm), _fwd(fwd), _rev(rev) {
pyeh9 0:cdd566529530 31
pyeh9 0:cdd566529530 32 // Set initial condition of PWM
pyeh9 0:cdd566529530 33 _pwm.period(0.001);
pyeh9 0:cdd566529530 34 _pwm = 0;
pyeh9 0:cdd566529530 35
pyeh9 0:cdd566529530 36 // Initial condition of output enables
pyeh9 0:cdd566529530 37 _fwd = 0;
pyeh9 0:cdd566529530 38 _rev = 0;
pyeh9 0:cdd566529530 39 }
pyeh9 0:cdd566529530 40
pyeh9 0:cdd566529530 41 // Motor with i2c encoder
pyeh9 0:cdd566529530 42 Motor::Motor(PinName pwm, PinName fwd, PinName rev, I2C *i):
pyeh9 0:cdd566529530 43 _pwm(pwm), _fwd(fwd), _rev(rev), _i2c(i) {
pyeh9 0:cdd566529530 44
pyeh9 0:cdd566529530 45 // Set initial condition of PWM
pyeh9 0:cdd566529530 46 _pwm.period(0.001);
pyeh9 0:cdd566529530 47 _pwm = 0;
pyeh9 0:cdd566529530 48
pyeh9 0:cdd566529530 49 // Initial condition of output enables
pyeh9 0:cdd566529530 50 _fwd = 0;
pyeh9 0:cdd566529530 51 _rev = 0;
pyeh9 0:cdd566529530 52
pyeh9 0:cdd566529530 53 ticks = 0;
pyeh9 0:cdd566529530 54 is_reversed = false;
pyeh9 0:cdd566529530 55 deviceAddress = 0x60;
pyeh9 0:cdd566529530 56 }
pyeh9 0:cdd566529530 57
pyeh9 0:cdd566529530 58 void Motor::speed(float speed) {
pyeh9 0:cdd566529530 59 _fwd = (speed > 0.0);
pyeh9 0:cdd566529530 60 _rev = (speed < 0.0);
pyeh9 0:cdd566529530 61 _pwm = abs(speed);
pyeh9 0:cdd566529530 62 }
pyeh9 0:cdd566529530 63
pyeh9 0:cdd566529530 64 void Motor::init(char address, int ID, bool term){
pyeh9 0:cdd566529530 65 char toWrite[] = {0x4D, address}; // writing to 0x4D tells encoder that you want to change its address
pyeh9 0:cdd566529530 66 _i2c->write(0x60, toWrite, 2);
pyeh9 0:cdd566529530 67 wait(0.01);
pyeh9 0:cdd566529530 68 // encoders are daisy-chained i2c devices. Writing to 0x4B tells the device you mean to put another after it
pyeh9 0:cdd566529530 69 if(!term){
pyeh9 0:cdd566529530 70 char disableTerm = 0x4B;
pyeh9 0:cdd566529530 71 _i2c->write(address, &disableTerm, 1);
pyeh9 0:cdd566529530 72 wait(0.01);
pyeh9 0:cdd566529530 73 }
pyeh9 0:cdd566529530 74 deviceAddress = address;
pyeh9 0:cdd566529530 75 motorID = ID;
pyeh9 0:cdd566529530 76 resetTicks();
pyeh9 0:cdd566529530 77 wait(0.01);
pyeh9 0:cdd566529530 78 }
pyeh9 0:cdd566529530 79
pyeh9 0:cdd566529530 80 void Motor::move(int pos){
pyeh9 0:cdd566529530 81 float diff1, spd1, lower = 0.09, upper = 0.1, integral1 = 0.0;
pyeh9 0:cdd566529530 82 //int K = 2612, Ki = 0.001;
pyeh9 0:cdd566529530 83 int K = 6500, Ki = 0.001;
pyeh9 0:cdd566529530 84 while (!(abs(float(ticks - pos)) <= 20))
pyeh9 0:cdd566529530 85 {
pyeh9 0:cdd566529530 86 diff1 = (pos - ticks);
pyeh9 0:cdd566529530 87 integral1 += (diff1*0.001);
pyeh9 0:cdd566529530 88 spd1 = (diff1/K) + (Ki*integral1);
pyeh9 0:cdd566529530 89 //rationalize a speed
pyeh9 0:cdd566529530 90 if (spd1 >= upper) spd1 = upper;
pyeh9 0:cdd566529530 91 else if(spd1 <= lower && spd1 > 0.0) spd1 = lower;
pyeh9 0:cdd566529530 92 else if(spd1 < 0.0 && spd1 >= -lower) spd1 = -lower;
pyeh9 0:cdd566529530 93 else if(spd1 <= -upper) spd1 = -upper;
pyeh9 0:cdd566529530 94 //printf("Motor %d is at position %d, moving with speed %f\n", motorID, ticks, spd1); // debugging
pyeh9 0:cdd566529530 95 speed(spd1);
pyeh9 0:cdd566529530 96 wait(0.001);
pyeh9 0:cdd566529530 97
pyeh9 0:cdd566529530 98 }//while
pyeh9 0:cdd566529530 99 speed(0);
pyeh9 0:cdd566529530 100 wait(0.5);
pyeh9 0:cdd566529530 101 // Adjusting
pyeh9 0:cdd566529530 102 while (!(abs(float(ticks - pos)) <= 10))
pyeh9 0:cdd566529530 103 {
pyeh9 0:cdd566529530 104 diff1 = (pos - ticks);
pyeh9 0:cdd566529530 105 spd1 = 0.04;
pyeh9 0:cdd566529530 106 //printf("Motor %d is at position %d, moving with speed %f\n", motorID, ticks, spd1); // debugging
pyeh9 0:cdd566529530 107 //rationalize a speed
pyeh9 0:cdd566529530 108 if(diff1 < 0) spd1 = -spd1;
pyeh9 0:cdd566529530 109 speed(spd1);
pyeh9 0:cdd566529530 110 wait(0.001);
pyeh9 0:cdd566529530 111
pyeh9 0:cdd566529530 112 }//while
pyeh9 0:cdd566529530 113 speed(0);
pyeh9 0:cdd566529530 114 wait(0.8);
pyeh9 0:cdd566529530 115 //printf("Done! Motor %d is at position %d\n", motorID, ticks); // debugging
pyeh9 0:cdd566529530 116 }
pyeh9 0:cdd566529530 117
pyeh9 0:cdd566529530 118 void Motor::getTicks(){
pyeh9 0:cdd566529530 119 char lsb, msb;
pyeh9 0:cdd566529530 120 char addr[2] = {0x40, 0x41}; // look on datasheet for this
pyeh9 0:cdd566529530 121 _i2c->write(deviceAddress, &addr[0], 1);
pyeh9 0:cdd566529530 122 _i2c->read(deviceAddress, &msb, 1);
pyeh9 0:cdd566529530 123 _i2c->write(deviceAddress, &addr[1], 1);
pyeh9 0:cdd566529530 124 _i2c->read(deviceAddress, &lsb, 1);
pyeh9 0:cdd566529530 125 ticks = (msb << 8) | lsb;
pyeh9 0:cdd566529530 126 if(is_reversed){
pyeh9 0:cdd566529530 127 if(ticks != 0) ticks = 65536-ticks;
pyeh9 0:cdd566529530 128 else ticks = 0;
pyeh9 0:cdd566529530 129 }
pyeh9 0:cdd566529530 130 if(ticks > 50000){ // When moving towards 0, you may overshoot and fill the buffer, and it will count backwards from 2^16 - 1.
pyeh9 0:cdd566529530 131 ticks = 0; // 50000 is rather arbitrary, it just lets me know I've gone backwards from 0. This isn't a great fix.
pyeh9 0:cdd566529530 132 }
pyeh9 0:cdd566529530 133 }
pyeh9 0:cdd566529530 134
pyeh9 0:cdd566529530 135 void Motor::resetTicks(){
pyeh9 0:cdd566529530 136 char reset = 0x4A;
pyeh9 0:cdd566529530 137 _i2c->write(deviceAddress, &reset, 1);
pyeh9 0:cdd566529530 138 }
pyeh9 0:cdd566529530 139
pyeh9 0:cdd566529530 140
pyeh9 0:cdd566529530 141