Peihsun Yeh
/
ECE4180FinalV4
CarlsenBot working
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?
User | Revision | Line number | New 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 |