Shuto Naruse / Mbed 2 deprecated eurobot_2011_main

Dependencies:   mbed PID

main.cpp@0:c8c04928d025, 2012-02-29 (annotated)

Committer:
narshu
Date:
Wed Feb 29 17:50:49 2012 +0000
Revision:
0:c8c04928d025
Primary Robot with Sonar function - needs PID tuning for motor control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:c8c04928d025 1 /**********************************************************************
narshu 0:c8c04928d025 2 * @file i2c_monitor.c
narshu 0:c8c04928d025 3 * @purpose Eurobot 2012 - Primary Robot MD25 Interface
narshu 0:c8c04928d025 4 * @version 0.2
narshu 0:c8c04928d025 5 * @date 13. Feb. 2012
narshu 0:c8c04928d025 6 * @author Crispian Poon
narshu 0:c8c04928d025 7 * @email pooncg@gmail.com
narshu 0:c8c04928d025 8 ______________________________________________________________________
narshu 0:c8c04928d025 9
narshu 0:c8c04928d025 10 Setup information:
narshu 0:c8c04928d025 11 1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA
narshu 0:c8c04928d025 12 2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable
narshu 0:c8c04928d025 13
narshu 0:c8c04928d025 14 **********************************************************************/
narshu 0:c8c04928d025 15
narshu 0:c8c04928d025 16 #include "mbed.h"
narshu 0:c8c04928d025 17 #include "PID.h"
narshu 0:c8c04928d025 18 //Sonar includes
narshu 0:c8c04928d025 19 #include "RF12B.h"
narshu 0:c8c04928d025 20 #include "SRF05.h"
narshu 0:c8c04928d025 21
narshu 0:c8c04928d025 22 //Constants declaration
narshu 0:c8c04928d025 23 const int md25Address = 0xB0;
narshu 0:c8c04928d025 24 const char cmdSetMotor1 = 0x00;
narshu 0:c8c04928d025 25 const char cmdSetMotor2 = 0x01;
narshu 0:c8c04928d025 26 const char cmdByte = 0x10;
narshu 0:c8c04928d025 27 const char cmdDisableAcceleration = 0x30;
narshu 0:c8c04928d025 28 const char cmdSetMode = 0x0F;
narshu 0:c8c04928d025 29 const char cmdResetEncoders = 0x20;
narshu 0:c8c04928d025 30 const char cmdGetEncoder1 = 0x02;
narshu 0:c8c04928d025 31 const char cmdGetEncoder2 = 0x06;
narshu 0:c8c04928d025 32 const int encoderRevCount = 1856;
narshu 0:c8c04928d025 33 const int wheelmm = 314;
narshu 0:c8c04928d025 34 const int robotCircumference = 1052;
narshu 0:c8c04928d025 35 #define RATE 0.01
narshu 0:c8c04928d025 36 //#include <queue>
narshu 0:c8c04928d025 37 #define CODE 0x88
narshu 0:c8c04928d025 38
narshu 0:c8c04928d025 39
narshu 0:c8c04928d025 40 //Global variables declaration
narshu 0:c8c04928d025 41 int tempByte;
narshu 0:c8c04928d025 42 PID PIDController1(0.9, 2, 0, RATE);
narshu 0:c8c04928d025 43
narshu 0:c8c04928d025 44 //Interface declaration
narshu 0:c8c04928d025 45 I2C i2c(p28, p27); // sda, scl
narshu 0:c8c04928d025 46 Serial pc(USBTX, USBRX); // tx, rx
narshu 0:c8c04928d025 47 DigitalOut OBLED1(LED1);
narshu 0:c8c04928d025 48 DigitalOut OBLED2(LED2);
narshu 0:c8c04928d025 49 SRF05 my_srf1(p13,p21);
narshu 0:c8c04928d025 50 SRF05 my_srf2(p13,p22);
narshu 0:c8c04928d025 51 SRF05 my_srf3(p13,p23);
narshu 0:c8c04928d025 52 RF12B RF_Robot(p5,p6,p7,p8,p9);
narshu 0:c8c04928d025 53
narshu 0:c8c04928d025 54 //Functions declaration
narshu 0:c8c04928d025 55 void resetEncoders();
narshu 0:c8c04928d025 56 long getEncoder1();
narshu 0:c8c04928d025 57 long getEncoder2();
narshu 0:c8c04928d025 58 void move(long distance, int speed);
narshu 0:c8c04928d025 59 void turn(int angle, int speed);
narshu 0:c8c04928d025 60 int getSignOfInt(int direction);
narshu 0:c8c04928d025 61 void stop();
narshu 0:c8c04928d025 62 void setSpeed(int speed);
narshu 0:c8c04928d025 63 void setSpeed(int speed1, int speed2);
narshu 0:c8c04928d025 64 void setMode(int mode);
narshu 0:c8c04928d025 65 long encoderToDistance(long encoder);
narshu 0:c8c04928d025 66 long distanceToEncoder(long distance);
narshu 0:c8c04928d025 67 void sendCommand(char command);
narshu 0:c8c04928d025 68 void sendCommand(char command1, char command2 );
narshu 0:c8c04928d025 69 long get4Bytes(char command);
narshu 0:c8c04928d025 70
narshu 0:c8c04928d025 71 //Main loop
narshu 0:c8c04928d025 72 int main() {
narshu 0:c8c04928d025 73 int Cmd = 30;
narshu 0:c8c04928d025 74
narshu 0:c8c04928d025 75
narshu 0:c8c04928d025 76 //PID controller initialisation
narshu 0:c8c04928d025 77 PIDController1.setMode(AUTO_MODE);
narshu 0:c8c04928d025 78 PIDController1.setInputLimits(0, 2500); //arbitarily set limit of 10000, can change!
narshu 0:c8c04928d025 79 PIDController1.setOutputLimits(0, 2500);
narshu 0:c8c04928d025 80
narshu 0:c8c04928d025 81
narshu 0:c8c04928d025 82 //TODO serial interface code, maybe with interrupt?
narshu 0:c8c04928d025 83
narshu 0:c8c04928d025 84 while (1) {
narshu 0:c8c04928d025 85
narshu 0:c8c04928d025 86 //Read serial to change variable
narshu 0:c8c04928d025 87 if (pc.readable() == 1) {
narshu 0:c8c04928d025 88 pc.scanf("%i", &Cmd);
narshu 0:c8c04928d025 89 }
narshu 0:c8c04928d025 90
narshu 0:c8c04928d025 91 //Tune PID referece
narshu 0:c8c04928d025 92 PIDController1.setTunings(0.4, 2, 0);
narshu 0:c8c04928d025 93
narshu 0:c8c04928d025 94 //Debug info
narshu 0:c8c04928d025 95 pc.printf("%i",Cmd);
narshu 0:c8c04928d025 96
narshu 0:c8c04928d025 97
narshu 0:c8c04928d025 98 move(500, Cmd);
narshu 0:c8c04928d025 99 wait_ms(2000);
narshu 0:c8c04928d025 100 //move(-200, 70);
narshu 0:c8c04928d025 101
narshu 0:c8c04928d025 102 }
narshu 0:c8c04928d025 103 }
narshu 0:c8c04928d025 104
narshu 0:c8c04928d025 105 //***************************************************************************************
narshu 0:c8c04928d025 106 //Primary robot specific instructions
narshu 0:c8c04928d025 107 //***************************************************************************************
narshu 0:c8c04928d025 108
narshu 0:c8c04928d025 109 float getSonar() {
narshu 0:c8c04928d025 110 float distance[3];
narshu 0:c8c04928d025 111 float result = 0;
narshu 0:c8c04928d025 112
narshu 0:c8c04928d025 113 RF_Robot.write(CODE);
narshu 0:c8c04928d025 114 my_srf1.trig();
narshu 0:c8c04928d025 115 wait_ms(50);
narshu 0:c8c04928d025 116
narshu 0:c8c04928d025 117 distance[0] = my_srf1.read();
narshu 0:c8c04928d025 118 distance[1] = my_srf2.read();
narshu 0:c8c04928d025 119 distance[2] = my_srf3.read();
narshu 0:c8c04928d025 120
narshu 0:c8c04928d025 121 for (int i = 0; i < 3; i++) {
narshu 0:c8c04928d025 122 if ( distance[i] > result) {
narshu 0:c8c04928d025 123 result = distance[i];
narshu 0:c8c04928d025 124 }
narshu 0:c8c04928d025 125 }
narshu 0:c8c04928d025 126 return result;
narshu 0:c8c04928d025 127 }
narshu 0:c8c04928d025 128
narshu 0:c8c04928d025 129 //*********************************************
narshu 0:c8c04928d025 130 //Parameter @distance in mm, @speed is 0 to 127
narshu 0:c8c04928d025 131 //Description: moves robot by @distance mm at @speed in straight line
narshu 0:c8c04928d025 132 //*********************************************
narshu 0:c8c04928d025 133 void move(long distance, int speed) {
narshu 0:c8c04928d025 134
narshu 0:c8c04928d025 135 long tempEndEncoder = 0;
narshu 0:c8c04928d025 136 long startEncoderCount = 0;
narshu 0:c8c04928d025 137 long previousEncoder1Count = 0;
narshu 0:c8c04928d025 138 long previousEncoder2Count = 0;
narshu 0:c8c04928d025 139 long tempEncoder1Speed = 0;
narshu 0:c8c04928d025 140 long tempEncoder2Speed = 0;
narshu 0:c8c04928d025 141 int directionFlag = 0;
narshu 0:c8c04928d025 142 float encoderSpeedConstant = 0;
narshu 0:c8c04928d025 143
narshu 0:c8c04928d025 144 //Variables initialisation
narshu 0:c8c04928d025 145 resetEncoders(); //reset MD25 encoder TODO too large distance may cause overflow
narshu 0:c8c04928d025 146 tempEndEncoder = distanceToEncoder(abs(distance)); //Convert distance to end encoder value
narshu 0:c8c04928d025 147 directionFlag = getSignOfInt(distance); //get direction from distance e.g. -1mm, +1mm converter to - and + operator
narshu 0:c8c04928d025 148
narshu 0:c8c04928d025 149 //setSpeed(speed);
narshu 0:c8c04928d025 150 //wait_ms(100);
narshu 0:c8c04928d025 151
narshu 0:c8c04928d025 152 while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu 0:c8c04928d025 153
narshu 0:c8c04928d025 154 //TODO loop counter
narshu 0:c8c04928d025 155
narshu 0:c8c04928d025 156 //calculate speed of encoder1 and encoder 2 change
narshu 0:c8c04928d025 157 tempEncoder1Speed = abs(getEncoder1() - previousEncoder1Count);
narshu 0:c8c04928d025 158 tempEncoder2Speed = abs(getEncoder2() - previousEncoder2Count);
narshu 0:c8c04928d025 159
narshu 0:c8c04928d025 160
narshu 0:c8c04928d025 161 //TODO optimize so only 1 calc is done per function call
narshu 0:c8c04928d025 162 //get encoder speed to @speed conversion ratio
narshu 0:c8c04928d025 163 encoderSpeedConstant = double(speed) / double(tempEncoder1Speed);
narshu 0:c8c04928d025 164
narshu 0:c8c04928d025 165 //set previous loop encoder values
narshu 0:c8c04928d025 166 previousEncoder1Count = getEncoder1();
narshu 0:c8c04928d025 167 previousEncoder2Count = getEncoder2();
narshu 0:c8c04928d025 168
narshu 0:c8c04928d025 169 //Set PID input value, tries to adjust encoder2 speed to encoder1 speed
narshu 0:c8c04928d025 170 PIDController1.setSetPoint(tempEncoder1Speed);
narshu 0:c8c04928d025 171 PIDController1.setProcessValue(tempEncoder2Speed);
narshu 0:c8c04928d025 172
narshu 0:c8c04928d025 173 float tempVal = PIDController1.compute();
narshu 0:c8c04928d025 174 //Debug Info
narshu 0:c8c04928d025 175 //pc.printf("%f \n", tempVal);
narshu 0:c8c04928d025 176 //PID control of real motor speeds, Motor2 actual speed mirros Motor1 actual speed
narshu 0:c8c04928d025 177 setSpeed(directionFlag*speed, directionFlag * tempVal * encoderSpeedConstant);
narshu 0:c8c04928d025 178
narshu 0:c8c04928d025 179 //delay to increase significant encoder counts
narshu 0:c8c04928d025 180 wait(RATE);
narshu 0:c8c04928d025 181
narshu 0:c8c04928d025 182 }
narshu 0:c8c04928d025 183
narshu 0:c8c04928d025 184 //Stop robot
narshu 0:c8c04928d025 185 stop();
narshu 0:c8c04928d025 186
narshu 0:c8c04928d025 187 //Debug info - see if loop is completed.
narshu 0:c8c04928d025 188 OBLED1 = true;
narshu 0:c8c04928d025 189
narshu 0:c8c04928d025 190 //Reset encoder values
narshu 0:c8c04928d025 191 resetEncoders();
narshu 0:c8c04928d025 192 }
narshu 0:c8c04928d025 193
narshu 0:c8c04928d025 194 void turn(int angle, int speed) {
narshu 0:c8c04928d025 195 resetEncoders();
narshu 0:c8c04928d025 196 long tempDistance = long((float(angle) / 360) * float(robotCircumference));
narshu 0:c8c04928d025 197 long tempEndEncoder = 0;
narshu 0:c8c04928d025 198 long startEncoderCount = 0;
narshu 0:c8c04928d025 199
narshu 0:c8c04928d025 200 tempEndEncoder = distanceToEncoder(abs(tempDistance));
narshu 0:c8c04928d025 201 startEncoderCount = getEncoder1();
narshu 0:c8c04928d025 202 setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);
narshu 0:c8c04928d025 203
narshu 0:c8c04928d025 204 while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {
narshu 0:c8c04928d025 205 setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);
narshu 0:c8c04928d025 206
narshu 0:c8c04928d025 207 }
narshu 0:c8c04928d025 208
narshu 0:c8c04928d025 209 resetEncoders();
narshu 0:c8c04928d025 210 stop();
narshu 0:c8c04928d025 211 }
narshu 0:c8c04928d025 212
narshu 0:c8c04928d025 213
narshu 0:c8c04928d025 214 //***************************************************************************************
narshu 0:c8c04928d025 215 //Primary robot specific helper functions
narshu 0:c8c04928d025 216 //***************************************************************************************
narshu 0:c8c04928d025 217
narshu 0:c8c04928d025 218 int getSignOfInt(int direction) {
narshu 0:c8c04928d025 219
narshu 0:c8c04928d025 220 direction = (direction < 0);
narshu 0:c8c04928d025 221
narshu 0:c8c04928d025 222 switch (direction) {
narshu 0:c8c04928d025 223 case 1:
narshu 0:c8c04928d025 224 return -1;
narshu 0:c8c04928d025 225 case 0:
narshu 0:c8c04928d025 226 return 1;
narshu 0:c8c04928d025 227 }
narshu 0:c8c04928d025 228
narshu 0:c8c04928d025 229 return 0;
narshu 0:c8c04928d025 230 }
narshu 0:c8c04928d025 231
narshu 0:c8c04928d025 232 long encoderToDistance(long encoder) {
narshu 0:c8c04928d025 233 return long((float(encoder) / float(encoderRevCount)) * wheelmm);
narshu 0:c8c04928d025 234 }
narshu 0:c8c04928d025 235
narshu 0:c8c04928d025 236 long distanceToEncoder(long distance) {
narshu 0:c8c04928d025 237 return long((float(distance) / float(wheelmm)) * encoderRevCount);
narshu 0:c8c04928d025 238 }
narshu 0:c8c04928d025 239
narshu 0:c8c04928d025 240
narshu 0:c8c04928d025 241 //***************************************************************************************
narshu 0:c8c04928d025 242 //MD25 instructions
narshu 0:c8c04928d025 243 //***************************************************************************************
narshu 0:c8c04928d025 244
narshu 0:c8c04928d025 245 void stop() {
narshu 0:c8c04928d025 246 sendCommand(cmdSetMotor1, 0);
narshu 0:c8c04928d025 247 sendCommand(cmdSetMotor2, 0);
narshu 0:c8c04928d025 248 }
narshu 0:c8c04928d025 249
narshu 0:c8c04928d025 250 void disableAcceleration() {
narshu 0:c8c04928d025 251 sendCommand(cmdByte, cmdDisableAcceleration);
narshu 0:c8c04928d025 252 }
narshu 0:c8c04928d025 253
narshu 0:c8c04928d025 254 void setSpeed(int speed) {
narshu 0:c8c04928d025 255 setMode(1);
narshu 0:c8c04928d025 256 disableAcceleration();
narshu 0:c8c04928d025 257 sendCommand(cmdSetMotor1, speed);
narshu 0:c8c04928d025 258 sendCommand(cmdSetMotor2, speed);
narshu 0:c8c04928d025 259 }
narshu 0:c8c04928d025 260
narshu 0:c8c04928d025 261 void setSpeed(int speed1, int speed2) {
narshu 0:c8c04928d025 262 setMode(1),
narshu 0:c8c04928d025 263 disableAcceleration();
narshu 0:c8c04928d025 264 sendCommand(cmdSetMotor1, speed1);
narshu 0:c8c04928d025 265 sendCommand(cmdSetMotor2, speed2);
narshu 0:c8c04928d025 266 }
narshu 0:c8c04928d025 267
narshu 0:c8c04928d025 268 void setMode(int mode) {
narshu 0:c8c04928d025 269 sendCommand(cmdSetMode, mode);
narshu 0:c8c04928d025 270 }
narshu 0:c8c04928d025 271
narshu 0:c8c04928d025 272 void resetEncoders() {
narshu 0:c8c04928d025 273 sendCommand(cmdByte, cmdResetEncoders) ;
narshu 0:c8c04928d025 274 }
narshu 0:c8c04928d025 275
narshu 0:c8c04928d025 276 long getEncoder1() {
narshu 0:c8c04928d025 277 return get4Bytes(cmdGetEncoder1);
narshu 0:c8c04928d025 278 }
narshu 0:c8c04928d025 279
narshu 0:c8c04928d025 280 long getEncoder2() {
narshu 0:c8c04928d025 281 return get4Bytes(cmdGetEncoder2);
narshu 0:c8c04928d025 282 }
narshu 0:c8c04928d025 283
narshu 0:c8c04928d025 284
narshu 0:c8c04928d025 285 //***************************************************************************************
narshu 0:c8c04928d025 286 //Abstract MD25 communication methods and functions
narshu 0:c8c04928d025 287 //***************************************************************************************
narshu 0:c8c04928d025 288
narshu 0:c8c04928d025 289 void sendCommand(char command) {
narshu 0:c8c04928d025 290 char buffer[1];
narshu 0:c8c04928d025 291 buffer[0] = command;
narshu 0:c8c04928d025 292 i2c.write(md25Address, &buffer[0], 1);
narshu 0:c8c04928d025 293 }
narshu 0:c8c04928d025 294
narshu 0:c8c04928d025 295 void sendCommand(char command1, char command2 ) {
narshu 0:c8c04928d025 296
narshu 0:c8c04928d025 297 char buffer[2];
narshu 0:c8c04928d025 298 buffer[0] = command1;
narshu 0:c8c04928d025 299 buffer[1] = command2;
narshu 0:c8c04928d025 300
narshu 0:c8c04928d025 301 i2c.write(md25Address, &buffer[0], 2);
narshu 0:c8c04928d025 302 }
narshu 0:c8c04928d025 303
narshu 0:c8c04928d025 304 long get4Bytes(char command) {
narshu 0:c8c04928d025 305 long tempWord = 0;
narshu 0:c8c04928d025 306 char cmd[4];
narshu 0:c8c04928d025 307
narshu 0:c8c04928d025 308 //i2c request
narshu 0:c8c04928d025 309 sendCommand(command);
narshu 0:c8c04928d025 310
narshu 0:c8c04928d025 311 //i2c read data back
narshu 0:c8c04928d025 312 i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25
narshu 0:c8c04928d025 313
narshu 0:c8c04928d025 314 //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off.
narshu 0:c8c04928d025 315
narshu 0:c8c04928d025 316 //First byte is largest, shift 4 bytes into tempWord
narshu 0:c8c04928d025 317 tempWord += cmd[0] << 24;
narshu 0:c8c04928d025 318 tempWord += cmd[1] << 16;
narshu 0:c8c04928d025 319 tempWord += cmd[2] << 8;
narshu 0:c8c04928d025 320 tempWord += cmd[3] ;
narshu 0:c8c04928d025 321
narshu 0:c8c04928d025 322 return tempWord;
narshu 0:c8c04928d025 323 }
narshu 0:c8c04928d025 324
narshu 0:c8c04928d025 325
narshu 0:c8c04928d025 326