Shuto Naruse
/
eurobot_2011_main
Diff: main.cpp
- Revision:
- 0:c8c04928d025
diff -r 000000000000 -r c8c04928d025 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Feb 29 17:50:49 2012 +0000 @@ -0,0 +1,326 @@ +/********************************************************************** + * @file i2c_monitor.c + * @purpose Eurobot 2012 - Primary Robot MD25 Interface + * @version 0.2 + * @date 13. Feb. 2012 + * @author Crispian Poon + * @email pooncg@gmail.com + ______________________________________________________________________ + + Setup information: + 1. Put pull up 2.2k resistors to +3.3v on I2C SCL and SDA + 2. Connect P28 SDA to MD25 yellow cable, P27 SCL to MD25 blue cable + + **********************************************************************/ + +#include "mbed.h" +#include "PID.h" +//Sonar includes +#include "RF12B.h" +#include "SRF05.h" + +//Constants declaration +const int md25Address = 0xB0; +const char cmdSetMotor1 = 0x00; +const char cmdSetMotor2 = 0x01; +const char cmdByte = 0x10; +const char cmdDisableAcceleration = 0x30; +const char cmdSetMode = 0x0F; +const char cmdResetEncoders = 0x20; +const char cmdGetEncoder1 = 0x02; +const char cmdGetEncoder2 = 0x06; +const int encoderRevCount = 1856; +const int wheelmm = 314; +const int robotCircumference = 1052; +#define RATE 0.01 +//#include <queue> +#define CODE 0x88 + + +//Global variables declaration +int tempByte; +PID PIDController1(0.9, 2, 0, RATE); + +//Interface declaration +I2C i2c(p28, p27); // sda, scl +Serial pc(USBTX, USBRX); // tx, rx +DigitalOut OBLED1(LED1); +DigitalOut OBLED2(LED2); +SRF05 my_srf1(p13,p21); +SRF05 my_srf2(p13,p22); +SRF05 my_srf3(p13,p23); +RF12B RF_Robot(p5,p6,p7,p8,p9); + +//Functions declaration +void resetEncoders(); +long getEncoder1(); +long getEncoder2(); +void move(long distance, int speed); +void turn(int angle, int speed); +int getSignOfInt(int direction); +void stop(); +void setSpeed(int speed); +void setSpeed(int speed1, int speed2); +void setMode(int mode); +long encoderToDistance(long encoder); +long distanceToEncoder(long distance); +void sendCommand(char command); +void sendCommand(char command1, char command2 ); +long get4Bytes(char command); + +//Main loop +int main() { + int Cmd = 30; + + + //PID controller initialisation + PIDController1.setMode(AUTO_MODE); + PIDController1.setInputLimits(0, 2500); //arbitarily set limit of 10000, can change! + PIDController1.setOutputLimits(0, 2500); + + + //TODO serial interface code, maybe with interrupt? + + while (1) { + + //Read serial to change variable + if (pc.readable() == 1) { + pc.scanf("%i", &Cmd); + } + + //Tune PID referece + PIDController1.setTunings(0.4, 2, 0); + + //Debug info + pc.printf("%i",Cmd); + + + move(500, Cmd); + wait_ms(2000); + //move(-200, 70); + + } +} + +//*************************************************************************************** +//Primary robot specific instructions +//*************************************************************************************** + +float getSonar() { + float distance[3]; + float result = 0; + + RF_Robot.write(CODE); + my_srf1.trig(); + wait_ms(50); + + distance[0] = my_srf1.read(); + distance[1] = my_srf2.read(); + distance[2] = my_srf3.read(); + + for (int i = 0; i < 3; i++) { + if ( distance[i] > result) { + result = distance[i]; + } + } + return result; +} + +//********************************************* +//Parameter @distance in mm, @speed is 0 to 127 +//Description: moves robot by @distance mm at @speed in straight line +//********************************************* +void move(long distance, int speed) { + + long tempEndEncoder = 0; + long startEncoderCount = 0; + long previousEncoder1Count = 0; + long previousEncoder2Count = 0; + long tempEncoder1Speed = 0; + long tempEncoder2Speed = 0; + int directionFlag = 0; + float encoderSpeedConstant = 0; + + //Variables initialisation + resetEncoders(); //reset MD25 encoder TODO too large distance may cause overflow + tempEndEncoder = distanceToEncoder(abs(distance)); //Convert distance to end encoder value + directionFlag = getSignOfInt(distance); //get direction from distance e.g. -1mm, +1mm converter to - and + operator + + //setSpeed(speed); + //wait_ms(100); + + while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) { + + //TODO loop counter + + //calculate speed of encoder1 and encoder 2 change + tempEncoder1Speed = abs(getEncoder1() - previousEncoder1Count); + tempEncoder2Speed = abs(getEncoder2() - previousEncoder2Count); + + + //TODO optimize so only 1 calc is done per function call + //get encoder speed to @speed conversion ratio + encoderSpeedConstant = double(speed) / double(tempEncoder1Speed); + + //set previous loop encoder values + previousEncoder1Count = getEncoder1(); + previousEncoder2Count = getEncoder2(); + + //Set PID input value, tries to adjust encoder2 speed to encoder1 speed + PIDController1.setSetPoint(tempEncoder1Speed); + PIDController1.setProcessValue(tempEncoder2Speed); + + float tempVal = PIDController1.compute(); + //Debug Info + //pc.printf("%f \n", tempVal); + //PID control of real motor speeds, Motor2 actual speed mirros Motor1 actual speed + setSpeed(directionFlag*speed, directionFlag * tempVal * encoderSpeedConstant); + + //delay to increase significant encoder counts + wait(RATE); + + } + + //Stop robot + stop(); + + //Debug info - see if loop is completed. + OBLED1 = true; + + //Reset encoder values + resetEncoders(); +} + +void turn(int angle, int speed) { + resetEncoders(); + long tempDistance = long((float(angle) / 360) * float(robotCircumference)); + long tempEndEncoder = 0; + long startEncoderCount = 0; + + tempEndEncoder = distanceToEncoder(abs(tempDistance)); + startEncoderCount = getEncoder1(); + setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed); + + while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) { + setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed); + + } + + resetEncoders(); + stop(); +} + + +//*************************************************************************************** +//Primary robot specific helper functions +//*************************************************************************************** + +int getSignOfInt(int direction) { + + direction = (direction < 0); + + switch (direction) { + case 1: + return -1; + case 0: + return 1; + } + + return 0; +} + +long encoderToDistance(long encoder) { + return long((float(encoder) / float(encoderRevCount)) * wheelmm); +} + +long distanceToEncoder(long distance) { + return long((float(distance) / float(wheelmm)) * encoderRevCount); +} + + +//*************************************************************************************** +//MD25 instructions +//*************************************************************************************** + +void stop() { + sendCommand(cmdSetMotor1, 0); + sendCommand(cmdSetMotor2, 0); +} + +void disableAcceleration() { + sendCommand(cmdByte, cmdDisableAcceleration); +} + +void setSpeed(int speed) { + setMode(1); + disableAcceleration(); + sendCommand(cmdSetMotor1, speed); + sendCommand(cmdSetMotor2, speed); +} + +void setSpeed(int speed1, int speed2) { + setMode(1), + disableAcceleration(); + sendCommand(cmdSetMotor1, speed1); + sendCommand(cmdSetMotor2, speed2); +} + +void setMode(int mode) { + sendCommand(cmdSetMode, mode); +} + +void resetEncoders() { + sendCommand(cmdByte, cmdResetEncoders) ; +} + +long getEncoder1() { + return get4Bytes(cmdGetEncoder1); +} + +long getEncoder2() { + return get4Bytes(cmdGetEncoder2); +} + + +//*************************************************************************************** +//Abstract MD25 communication methods and functions +//*************************************************************************************** + +void sendCommand(char command) { + char buffer[1]; + buffer[0] = command; + i2c.write(md25Address, &buffer[0], 1); +} + +void sendCommand(char command1, char command2 ) { + + char buffer[2]; + buffer[0] = command1; + buffer[1] = command2; + + i2c.write(md25Address, &buffer[0], 2); +} + +long get4Bytes(char command) { + long tempWord = 0; + char cmd[4]; + + //i2c request + sendCommand(command); + + //i2c read data back + i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25 + + //FIXED 22FEB2012 CRISPIAN Taken 0.07 delay off. + + //First byte is largest, shift 4 bytes into tempWord + tempWord += cmd[0] << 24; + tempWord += cmd[1] << 16; + tempWord += cmd[2] << 8; + tempWord += cmd[3] ; + + return tempWord; +} + + +