Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

motors/motors.cpp@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Commit before publishing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 1:cc2a9eb0bd55 1 /**********************************************************************
narshu 1:cc2a9eb0bd55 2 * @file motors.cpp
narshu 1:cc2a9eb0bd55 3 * @purpose Eurobot 2012 - Secondary Robot MD25 Interface
narshu 1:cc2a9eb0bd55 4 * @version 0.2
narshu 1:cc2a9eb0bd55 5 * @date 4th April 2012
narshu 1:cc2a9eb0bd55 6 * @author Crispian Poon
narshu 1:cc2a9eb0bd55 7 * @email pooncg@gmail.com
narshu 1:cc2a9eb0bd55 8 ______________________________________________________________________
narshu 1:cc2a9eb0bd55 9
narshu 1:cc2a9eb0bd55 10 Setup information:
narshu 1:cc2a9eb0bd55 11 1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA
narshu 1:cc2a9eb0bd55 12 2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable
narshu 1:cc2a9eb0bd55 13
narshu 1:cc2a9eb0bd55 14 **********************************************************************/
narshu 1:cc2a9eb0bd55 15
narshu 1:cc2a9eb0bd55 16 #include "mbed.h"
narshu 1:cc2a9eb0bd55 17 #include "motors.h"
narshu 1:cc2a9eb0bd55 18 #include "globals.h"
narshu 1:cc2a9eb0bd55 19 #include "TSH.h"
narshu 1:cc2a9eb0bd55 20
narshu 1:cc2a9eb0bd55 21 Motors::Motors(TSI2C &i2cin) : i2c(i2cin) {
narshu 1:cc2a9eb0bd55 22
narshu 1:cc2a9eb0bd55 23 }
narshu 1:cc2a9eb0bd55 24
narshu 1:cc2a9eb0bd55 25 //***************************************************************************************
narshu 1:cc2a9eb0bd55 26 //Secondary robot specific instructions
narshu 1:cc2a9eb0bd55 27 //***************************************************************************************
narshu 1:cc2a9eb0bd55 28
narshu 1:cc2a9eb0bd55 29 void Motors::move(int distance, int speed) {
narshu 1:cc2a9eb0bd55 30 //resetEncoders(); TODO use kalman as feedback instead!
narshu 1:cc2a9eb0bd55 31
narshu 1:cc2a9eb0bd55 32 int tempEndEncoder = 0;
narshu 1:cc2a9eb0bd55 33 int startEncoderCount = 0;
narshu 1:cc2a9eb0bd55 34
narshu 1:cc2a9eb0bd55 35 tempEndEncoder = distanceToEncoder(abs(distance));
narshu 1:cc2a9eb0bd55 36 startEncoderCount = getEncoder1();
narshu 1:cc2a9eb0bd55 37
narshu 1:cc2a9eb0bd55 38 setSpeed(getSignOfInt(distance) * speed);
narshu 1:cc2a9eb0bd55 39
narshu 1:cc2a9eb0bd55 40 while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu 1:cc2a9eb0bd55 41 setSpeed(getSignOfInt(distance) * speed);
narshu 1:cc2a9eb0bd55 42 }
narshu 1:cc2a9eb0bd55 43
narshu 1:cc2a9eb0bd55 44 //resetEncoders();
narshu 1:cc2a9eb0bd55 45 stop();
narshu 1:cc2a9eb0bd55 46 }
narshu 1:cc2a9eb0bd55 47
narshu 1:cc2a9eb0bd55 48 void Motors::turn(int angle, int speed) {
narshu 1:cc2a9eb0bd55 49 //resetEncoders(); TODO use kalman as feedback instead!
narshu 1:cc2a9eb0bd55 50 int tempDistance = int((float(angle) / 360) * float(robotCircumference));
narshu 1:cc2a9eb0bd55 51 int tempEndEncoder = 0;
narshu 1:cc2a9eb0bd55 52 int startEncoderCount = 0;
narshu 1:cc2a9eb0bd55 53
narshu 1:cc2a9eb0bd55 54 tempEndEncoder = distanceToEncoder(abs(tempDistance));
narshu 1:cc2a9eb0bd55 55 startEncoderCount = getEncoder1();
narshu 1:cc2a9eb0bd55 56 setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
narshu 1:cc2a9eb0bd55 57
narshu 1:cc2a9eb0bd55 58 while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu 1:cc2a9eb0bd55 59 setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
narshu 1:cc2a9eb0bd55 60
narshu 1:cc2a9eb0bd55 61 }
narshu 1:cc2a9eb0bd55 62
narshu 1:cc2a9eb0bd55 63 //resetEncoders();
narshu 1:cc2a9eb0bd55 64 stop();
narshu 1:cc2a9eb0bd55 65 }
narshu 1:cc2a9eb0bd55 66
narshu 1:cc2a9eb0bd55 67 //***************************************************************************************
narshu 1:cc2a9eb0bd55 68 //Secondary robot specific helper functions
narshu 1:cc2a9eb0bd55 69 //***************************************************************************************
narshu 1:cc2a9eb0bd55 70
narshu 1:cc2a9eb0bd55 71
narshu 1:cc2a9eb0bd55 72 int Motors::getSignOfInt(int direction) {
narshu 1:cc2a9eb0bd55 73
narshu 1:cc2a9eb0bd55 74 direction = (direction < 0);
narshu 1:cc2a9eb0bd55 75
narshu 1:cc2a9eb0bd55 76 switch (direction) {
narshu 1:cc2a9eb0bd55 77 case 1:
narshu 1:cc2a9eb0bd55 78 return -1;
narshu 1:cc2a9eb0bd55 79 case 0:
narshu 1:cc2a9eb0bd55 80 return 1;
narshu 1:cc2a9eb0bd55 81 }
narshu 1:cc2a9eb0bd55 82
narshu 1:cc2a9eb0bd55 83 return 0;
narshu 1:cc2a9eb0bd55 84 }
narshu 1:cc2a9eb0bd55 85
narshu 1:cc2a9eb0bd55 86 // returns distance in mm.
narshu 1:cc2a9eb0bd55 87 float Motors::encoderToDistance(int encoder) {
narshu 1:cc2a9eb0bd55 88 return (float(encoder) / float(encoderRevCount)) * wheelmm;
narshu 1:cc2a9eb0bd55 89 }
narshu 1:cc2a9eb0bd55 90
narshu 1:cc2a9eb0bd55 91 int Motors::distanceToEncoder(float distance) {
narshu 1:cc2a9eb0bd55 92 return int((distance / float(wheelmm)) * encoderRevCount);
narshu 1:cc2a9eb0bd55 93 }
narshu 1:cc2a9eb0bd55 94
narshu 1:cc2a9eb0bd55 95
narshu 1:cc2a9eb0bd55 96 //***************************************************************************************
narshu 1:cc2a9eb0bd55 97 //MD25 instructions
narshu 1:cc2a9eb0bd55 98 //***************************************************************************************
narshu 1:cc2a9eb0bd55 99
narshu 1:cc2a9eb0bd55 100 void Motors::stop() {
narshu 1:cc2a9eb0bd55 101 sendCommand(cmdSetMotor1, 0);
narshu 1:cc2a9eb0bd55 102 sendCommand(cmdSetMotor2, 0);
narshu 1:cc2a9eb0bd55 103 }
narshu 1:cc2a9eb0bd55 104
narshu 1:cc2a9eb0bd55 105 void Motors::setSpeed(int speed) {
narshu 1:cc2a9eb0bd55 106 setMode(1);
narshu 1:cc2a9eb0bd55 107 //sendCommand(cmdSetAcceleration, 0x02);
narshu 1:cc2a9eb0bd55 108 ///sendCommand(cmdByte, 0x30);
narshu 1:cc2a9eb0bd55 109 sendCommand(cmdSetMotor1, speed);
narshu 1:cc2a9eb0bd55 110 sendCommand(cmdSetMotor2, speed);
narshu 1:cc2a9eb0bd55 111 }
narshu 1:cc2a9eb0bd55 112
narshu 1:cc2a9eb0bd55 113 void Motors::setSpeed(int speed1, int speed2) {
narshu 1:cc2a9eb0bd55 114 setMode(1),
narshu 1:cc2a9eb0bd55 115 //sendCommand(cmdSetAcceleration, 0x02);
narshu 1:cc2a9eb0bd55 116 // sendCommand(cmdByte, 0x30);
narshu 1:cc2a9eb0bd55 117 sendCommand(cmdSetMotor1, speed1);
narshu 1:cc2a9eb0bd55 118 sendCommand(cmdSetMotor2, speed2);
narshu 1:cc2a9eb0bd55 119 }
narshu 1:cc2a9eb0bd55 120
narshu 1:cc2a9eb0bd55 121 void Motors::setMode(int mode) {
narshu 1:cc2a9eb0bd55 122 sendCommand(cmdSetMode, mode);
narshu 1:cc2a9eb0bd55 123 }
narshu 1:cc2a9eb0bd55 124
narshu 1:cc2a9eb0bd55 125 void Motors::resetEncoders() {
narshu 1:cc2a9eb0bd55 126 sendCommand(cmdByte, cmdResetEncoders);
narshu 1:cc2a9eb0bd55 127 }
narshu 1:cc2a9eb0bd55 128
narshu 1:cc2a9eb0bd55 129 int Motors::getEncoder1() {
narshu 1:cc2a9eb0bd55 130 return get4Bytes(cmdGetEncoder1);
narshu 1:cc2a9eb0bd55 131 }
narshu 1:cc2a9eb0bd55 132
narshu 1:cc2a9eb0bd55 133 int Motors::getEncoder2() {
narshu 1:cc2a9eb0bd55 134 return get4Bytes(cmdGetEncoder2);
narshu 1:cc2a9eb0bd55 135 }
narshu 1:cc2a9eb0bd55 136
narshu 1:cc2a9eb0bd55 137 void Motors::disableAcceleration() {
narshu 1:cc2a9eb0bd55 138 sendCommand(cmdByte, cmdDisableAcceleration);
narshu 1:cc2a9eb0bd55 139 }
narshu 1:cc2a9eb0bd55 140
narshu 1:cc2a9eb0bd55 141
narshu 1:cc2a9eb0bd55 142
narshu 1:cc2a9eb0bd55 143 //***************************************************************************************
narshu 1:cc2a9eb0bd55 144 //Abstract MD25 communication methods and functions
narshu 1:cc2a9eb0bd55 145 //***************************************************************************************
narshu 1:cc2a9eb0bd55 146
narshu 1:cc2a9eb0bd55 147 int Motors::get4Bytes(char command) {
narshu 1:cc2a9eb0bd55 148 long tempWord = 0;
narshu 1:cc2a9eb0bd55 149 char cmd[4];
narshu 1:cc2a9eb0bd55 150
narshu 1:cc2a9eb0bd55 151 //i2c request
narshu 1:cc2a9eb0bd55 152 sendCommand(command);
narshu 1:cc2a9eb0bd55 153
narshu 1:cc2a9eb0bd55 154 //i2c read data back
narshu 1:cc2a9eb0bd55 155 i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
narshu 1:cc2a9eb0bd55 156
narshu 1:cc2a9eb0bd55 157 //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.
narshu 1:cc2a9eb0bd55 158
narshu 1:cc2a9eb0bd55 159 //First byte is largest, shift 4 bytes into tempWord
narshu 1:cc2a9eb0bd55 160 tempWord += cmd[0] << 24;
narshu 1:cc2a9eb0bd55 161 tempWord += cmd[1] << 16;
narshu 1:cc2a9eb0bd55 162 tempWord += cmd[2] << 8;
narshu 1:cc2a9eb0bd55 163 tempWord += cmd[3] ;
narshu 1:cc2a9eb0bd55 164
narshu 1:cc2a9eb0bd55 165 return tempWord;
narshu 1:cc2a9eb0bd55 166 }
narshu 1:cc2a9eb0bd55 167
narshu 1:cc2a9eb0bd55 168 void Motors::sendCommand(char command) {
narshu 1:cc2a9eb0bd55 169 char buffer[1];
narshu 1:cc2a9eb0bd55 170 buffer[0] = command;
narshu 1:cc2a9eb0bd55 171 i2c.write(md25Address, &buffer[0], 1);
narshu 1:cc2a9eb0bd55 172 }
narshu 1:cc2a9eb0bd55 173
narshu 1:cc2a9eb0bd55 174 void Motors::sendCommand(char command1, char command2 ) {
narshu 1:cc2a9eb0bd55 175
narshu 1:cc2a9eb0bd55 176 char buffer[2];
narshu 1:cc2a9eb0bd55 177 buffer[0] = command1;
narshu 1:cc2a9eb0bd55 178 buffer[1] = command2;
narshu 1:cc2a9eb0bd55 179
narshu 1:cc2a9eb0bd55 180 i2c.write(md25Address, &buffer[0], 2);
narshu 1:cc2a9eb0bd55 181 }
narshu 1:cc2a9eb0bd55 182
narshu 1:cc2a9eb0bd55 183 void Motors::coastStop(void){
narshu 1:cc2a9eb0bd55 184 stop();
narshu 1:cc2a9eb0bd55 185 }