Diff: main.cpp
- Revision:
- 0:c8c04928d025
--- /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;
+}
+
+
+
Shuto Naruse