Shuto Naruse
/
eurobot_2011_main_PID
main.cpp
- Committer:
- narshu
- Date:
- 2012-03-14
- Revision:
- 0:dc84eed6b8b6
File content as of revision 0:dc84eed6b8b6:
/********************************************************************** * @file i2c_monitor.c * @purpose MD25 Interface * @version 0.1 * @date 25. Jan. 2012 * @author Crispian Poon * @email pooncg@gmail.com **********************************************************************/ #include "mbed.h" //Constants declaration const int md25Address = 0xB0; const char cmdSetMotor1 = 0x00; const char cmdSetMotor2 = 0x01; const char cmdByte = 0x10; 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; //Global variables declaration int tempByte; //Interface declaration I2C i2c(p9, p10); // sda, scl Serial pc(USBTX, USBRX); // tx, rx DigitalOut OBLED1(LED1); DigitalOut OBLED2(LED2); //Functions declaration void resetEncoders(); long getEncoder1(); 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 ); //Main loop int main() { //TODO declare serial //setSpeed(-127); //delay(5000); //setSpeed(0); //Serial.println(getEncoder1(), DEC); //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! while (1) { move(180, 30); wait_ms(3000); stop(); turn(-180, 30); OBLED1 = true; } } void resetEncoders() { sendCommand(cmdByte, cmdResetEncoders) ; } long getEncoder1() { long tempEncoderCount = 0; char cmd[4]; //i2c request encoder position 1 sendCommand(cmdGetEncoder1); //TODO create abstract function for read command //i2c read data back i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25 tempEncoderCount += cmd[0] << 24; tempEncoderCount += cmd[1] << 16; tempEncoderCount += cmd[2] << 8; tempEncoderCount += cmd[3] ; return tempEncoderCount; } long getEncoder2() { long tempEncoderCount = 0; char cmd[4]; //i2c request encoder position 1 sendCommand(cmdGetEncoder2); //i2c read data back i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25 tempEncoderCount += cmd[0] << 24; tempEncoderCount += cmd[1] << 16; tempEncoderCount += cmd[2] << 8; tempEncoderCount += cmd[3] ; return tempEncoderCount; } void move(long distance, int speed) { resetEncoders(); long tempEndEncoder = 0; long startEncoderCount = 0; tempEndEncoder = distanceToEncoder(abs(distance)); startEncoderCount = getEncoder1(); setSpeed(getSignOfInt(distance) * speed); while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) { setSpeed(getSignOfInt(distance) * speed); } resetEncoders(); stop(); } 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(); } int getSignOfInt(int direction) { direction = (direction < 0); switch (direction) { case 1: return -1; case 0: return 1; } return 0; } void stop() { sendCommand(cmdSetMotor1, 0); sendCommand(cmdSetMotor2, 0); } void setSpeed(int speed) { setMode(1); ///sendCommand(cmdByte, 0x30); sendCommand(cmdSetMotor1, speed); sendCommand(cmdSetMotor2, speed); } void setSpeed(int speed1, int speed2) { setMode(1), // sendCommand(cmdByte, 0x30); sendCommand(cmdSetMotor1, speed1); sendCommand(cmdSetMotor2, speed2); } void setMode(int mode) { sendCommand(cmdSetMode, mode); } long encoderToDistance(long encoder) { return long((float(encoder) / float(encoderRevCount)) * wheelmm); } long distanceToEncoder(long distance) { return long((float(distance) / float(wheelmm)) * encoderRevCount); } 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); }