Shuto Naruse / Mbed 2 deprecated eurobot_2011_main_PID

Dependencies:   mbed

main.cpp@0:dc84eed6b8b6, 2012-03-14 (annotated)

Committer:
narshu
Date:
Wed Mar 14 17:58:49 2012 +0000
Revision:
0:dc84eed6b8b6

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


narshu

0:dc84eed6b8b6


     1

/**********************************************************************





narshu

0:dc84eed6b8b6


     2

 * @file        i2c_monitor.c





narshu

0:dc84eed6b8b6


     3

 * @purpose     MD25 Interface





narshu

0:dc84eed6b8b6


     4

 * @version     0.1





narshu

0:dc84eed6b8b6


     5

 * @date        25. Jan. 2012





narshu

0:dc84eed6b8b6


     6

 * @author      Crispian Poon





narshu

0:dc84eed6b8b6


     7

 * @email       pooncg@gmail.com





narshu

0:dc84eed6b8b6


     8

 **********************************************************************/





narshu

0:dc84eed6b8b6


     9







narshu

0:dc84eed6b8b6


    10

#include "mbed.h"





narshu

0:dc84eed6b8b6


    11







narshu

0:dc84eed6b8b6


    12

//Constants declaration





narshu

0:dc84eed6b8b6


    13

const int md25Address = 0xB0;





narshu

0:dc84eed6b8b6


    14

const char cmdSetMotor1 = 0x00;





narshu

0:dc84eed6b8b6


    15

const char cmdSetMotor2 = 0x01;





narshu

0:dc84eed6b8b6


    16

const char cmdByte = 0x10;





narshu

0:dc84eed6b8b6


    17

const char cmdSetMode = 0x0F;





narshu

0:dc84eed6b8b6


    18

const char cmdResetEncoders = 0x20;





narshu

0:dc84eed6b8b6


    19

const char cmdGetEncoder1 = 0x02;





narshu

0:dc84eed6b8b6


    20

const char cmdGetEncoder2 = 0x06;





narshu

0:dc84eed6b8b6


    21

const int encoderRevCount = 1856;





narshu

0:dc84eed6b8b6


    22

const int wheelmm = 314;





narshu

0:dc84eed6b8b6


    23

const int robotCircumference = 1052;





narshu

0:dc84eed6b8b6


    24







narshu

0:dc84eed6b8b6


    25







narshu

0:dc84eed6b8b6


    26

//Global variables declaration





narshu

0:dc84eed6b8b6


    27

int tempByte;





narshu

0:dc84eed6b8b6


    28







narshu

0:dc84eed6b8b6


    29

//Interface declaration





narshu

0:dc84eed6b8b6


    30

I2C i2c(p9, p10);        // sda, scl





narshu

0:dc84eed6b8b6


    31

Serial pc(USBTX, USBRX); // tx, rx





narshu

0:dc84eed6b8b6


    32

DigitalOut     OBLED1(LED1);





narshu

0:dc84eed6b8b6


    33

DigitalOut     OBLED2(LED2);





narshu

0:dc84eed6b8b6


    34







narshu

0:dc84eed6b8b6


    35

//Functions declaration





narshu

0:dc84eed6b8b6


    36

void resetEncoders();





narshu

0:dc84eed6b8b6


    37

long getEncoder1();





narshu

0:dc84eed6b8b6


    38

void move(long distance, int speed);





narshu

0:dc84eed6b8b6


    39

void turn(int angle, int speed);





narshu

0:dc84eed6b8b6


    40

int getSignOfInt(int direction);





narshu

0:dc84eed6b8b6


    41

void stop();





narshu

0:dc84eed6b8b6


    42

void setSpeed(int speed);





narshu

0:dc84eed6b8b6


    43

void setSpeed(int speed1, int speed2);





narshu

0:dc84eed6b8b6


    44

void setMode(int mode);





narshu

0:dc84eed6b8b6


    45

long encoderToDistance(long encoder);





narshu

0:dc84eed6b8b6


    46

long distanceToEncoder(long distance);





narshu

0:dc84eed6b8b6


    47

void sendCommand(char command);





narshu

0:dc84eed6b8b6


    48

void sendCommand(char command1, char command2 );





narshu

0:dc84eed6b8b6


    49







narshu

0:dc84eed6b8b6


    50

//Main loop





narshu

0:dc84eed6b8b6


    51

int main() {





narshu

0:dc84eed6b8b6


    52

    //TODO declare serial





narshu

0:dc84eed6b8b6


    53

    //setSpeed(-127);





narshu

0:dc84eed6b8b6


    54

    //delay(5000);





narshu

0:dc84eed6b8b6


    55

    //setSpeed(0);





narshu

0:dc84eed6b8b6


    56

    //Serial.println(getEncoder1(), DEC);





narshu

0:dc84eed6b8b6


    57

    





narshu

0:dc84eed6b8b6


    58

    //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!





narshu

0:dc84eed6b8b6


    59

    while (1) {





narshu

0:dc84eed6b8b6


    60

       move(180, 30);





narshu

0:dc84eed6b8b6


    61

       wait_ms(3000);





narshu

0:dc84eed6b8b6


    62

       stop();





narshu

0:dc84eed6b8b6


    63

       turn(-180, 30);





narshu

0:dc84eed6b8b6


    64

        OBLED1 = true;





narshu

0:dc84eed6b8b6


    65

    }





narshu

0:dc84eed6b8b6


    66

}





narshu

0:dc84eed6b8b6


    67







narshu

0:dc84eed6b8b6


    68

void resetEncoders() {





narshu

0:dc84eed6b8b6


    69







narshu

0:dc84eed6b8b6


    70

    sendCommand(cmdByte, cmdResetEncoders) ;





narshu

0:dc84eed6b8b6


    71







narshu

0:dc84eed6b8b6


    72

}





narshu

0:dc84eed6b8b6


    73







narshu

0:dc84eed6b8b6


    74

long getEncoder1() {





narshu

0:dc84eed6b8b6


    75

    long tempEncoderCount = 0;





narshu

0:dc84eed6b8b6


    76

    char cmd[4];





narshu

0:dc84eed6b8b6


    77







narshu

0:dc84eed6b8b6


    78

    //i2c request encoder position 1





narshu

0:dc84eed6b8b6


    79

    sendCommand(cmdGetEncoder1);





narshu

0:dc84eed6b8b6


    80







narshu

0:dc84eed6b8b6


    81

    //TODO create abstract function for read command





narshu

0:dc84eed6b8b6


    82

    //i2c read data back





narshu

0:dc84eed6b8b6


    83

    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25





narshu

0:dc84eed6b8b6


    84







narshu

0:dc84eed6b8b6


    85

    tempEncoderCount += cmd[0] << 24;





narshu

0:dc84eed6b8b6


    86

    tempEncoderCount += cmd[1] << 16;





narshu

0:dc84eed6b8b6


    87

    tempEncoderCount += cmd[2] << 8;





narshu

0:dc84eed6b8b6


    88

    tempEncoderCount += cmd[3] ;





narshu

0:dc84eed6b8b6


    89







narshu

0:dc84eed6b8b6


    90

    return tempEncoderCount;





narshu

0:dc84eed6b8b6


    91







narshu

0:dc84eed6b8b6


    92

}





narshu

0:dc84eed6b8b6


    93







narshu

0:dc84eed6b8b6


    94

long getEncoder2() {





narshu

0:dc84eed6b8b6


    95

    long tempEncoderCount = 0;





narshu

0:dc84eed6b8b6


    96

    char cmd[4];





narshu

0:dc84eed6b8b6


    97







narshu

0:dc84eed6b8b6


    98

    //i2c request encoder position 1





narshu

0:dc84eed6b8b6


    99

    sendCommand(cmdGetEncoder2);





narshu

0:dc84eed6b8b6


   100







narshu

0:dc84eed6b8b6


   101

    //i2c read data back





narshu

0:dc84eed6b8b6


   102

    i2c.read(md25Address, cmd, 4);// Request 4 bytes from MD25





narshu

0:dc84eed6b8b6


   103







narshu

0:dc84eed6b8b6


   104

    tempEncoderCount += cmd[0] << 24;





narshu

0:dc84eed6b8b6


   105

    tempEncoderCount += cmd[1] << 16;





narshu

0:dc84eed6b8b6


   106

    tempEncoderCount += cmd[2] << 8;





narshu

0:dc84eed6b8b6


   107

    tempEncoderCount += cmd[3] ;





narshu

0:dc84eed6b8b6


   108







narshu

0:dc84eed6b8b6


   109

    return tempEncoderCount;





narshu

0:dc84eed6b8b6


   110







narshu

0:dc84eed6b8b6


   111







narshu

0:dc84eed6b8b6


   112

}





narshu

0:dc84eed6b8b6


   113







narshu

0:dc84eed6b8b6


   114

void move(long distance, int speed) {





narshu

0:dc84eed6b8b6


   115

    resetEncoders();





narshu

0:dc84eed6b8b6


   116

    long tempEndEncoder = 0;





narshu

0:dc84eed6b8b6


   117

    long startEncoderCount = 0;





narshu

0:dc84eed6b8b6


   118







narshu

0:dc84eed6b8b6


   119

    tempEndEncoder = distanceToEncoder(abs(distance));





narshu

0:dc84eed6b8b6


   120

    startEncoderCount = getEncoder1();





narshu

0:dc84eed6b8b6


   121







narshu

0:dc84eed6b8b6


   122

    setSpeed(getSignOfInt(distance) * speed);





narshu

0:dc84eed6b8b6


   123







narshu

0:dc84eed6b8b6


   124

    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {





narshu

0:dc84eed6b8b6


   125

        setSpeed(getSignOfInt(distance) * speed);





narshu

0:dc84eed6b8b6


   126

    }





narshu

0:dc84eed6b8b6


   127







narshu

0:dc84eed6b8b6


   128







narshu

0:dc84eed6b8b6


   129

    resetEncoders();





narshu

0:dc84eed6b8b6


   130

    stop();





narshu

0:dc84eed6b8b6


   131

}





narshu

0:dc84eed6b8b6


   132







narshu

0:dc84eed6b8b6


   133

void turn(int angle, int speed) {





narshu

0:dc84eed6b8b6


   134

    resetEncoders();





narshu

0:dc84eed6b8b6


   135

    long tempDistance = long((float(angle) / 360) * float(robotCircumference));





narshu

0:dc84eed6b8b6


   136

    long tempEndEncoder = 0;





narshu

0:dc84eed6b8b6


   137

    long startEncoderCount = 0;





narshu

0:dc84eed6b8b6


   138







narshu

0:dc84eed6b8b6


   139

    tempEndEncoder = distanceToEncoder(abs(tempDistance));





narshu

0:dc84eed6b8b6


   140

    startEncoderCount = getEncoder1();





narshu

0:dc84eed6b8b6


   141

    setSpeed(getSignOfInt(tempDistance) * speed, -getSignOfInt(tempDistance) * speed);





narshu

0:dc84eed6b8b6


   142







narshu

0:dc84eed6b8b6


   143

    while (abs(getEncoder1() - startEncoderCount) < tempEndEncoder) {





narshu

0:dc84eed6b8b6


   144

        setSpeed(getSignOfInt(tempDistance) * speed,-getSignOfInt(tempDistance) * speed);





narshu

0:dc84eed6b8b6


   145







narshu

0:dc84eed6b8b6


   146

    }





narshu

0:dc84eed6b8b6


   147

    





narshu

0:dc84eed6b8b6


   148

    resetEncoders();





narshu

0:dc84eed6b8b6


   149

    stop();





narshu

0:dc84eed6b8b6


   150

}





narshu

0:dc84eed6b8b6


   151







narshu

0:dc84eed6b8b6


   152







narshu

0:dc84eed6b8b6


   153

int getSignOfInt(int direction) {





narshu

0:dc84eed6b8b6


   154







narshu

0:dc84eed6b8b6


   155

    direction = (direction < 0);





narshu

0:dc84eed6b8b6


   156







narshu

0:dc84eed6b8b6


   157

    switch (direction) {





narshu

0:dc84eed6b8b6


   158

        case 1:





narshu

0:dc84eed6b8b6


   159

            return -1;





narshu

0:dc84eed6b8b6


   160

        case 0:





narshu

0:dc84eed6b8b6


   161

            return 1;





narshu

0:dc84eed6b8b6


   162

    }





narshu

0:dc84eed6b8b6


   163

    





narshu

0:dc84eed6b8b6


   164

    return 0;





narshu

0:dc84eed6b8b6


   165

}





narshu

0:dc84eed6b8b6


   166







narshu

0:dc84eed6b8b6


   167

void stop() {





narshu

0:dc84eed6b8b6


   168

    sendCommand(cmdSetMotor1, 0);





narshu

0:dc84eed6b8b6


   169

    sendCommand(cmdSetMotor2, 0);





narshu

0:dc84eed6b8b6


   170

}





narshu

0:dc84eed6b8b6


   171







narshu

0:dc84eed6b8b6


   172

void setSpeed(int speed) {





narshu

0:dc84eed6b8b6


   173

    setMode(1);





narshu

0:dc84eed6b8b6


   174

    ///sendCommand(cmdByte, 0x30);





narshu

0:dc84eed6b8b6


   175

    sendCommand(cmdSetMotor1, speed);





narshu

0:dc84eed6b8b6


   176

    sendCommand(cmdSetMotor2, speed);





narshu

0:dc84eed6b8b6


   177

}





narshu

0:dc84eed6b8b6


   178







narshu

0:dc84eed6b8b6


   179

void setSpeed(int speed1, int speed2) {





narshu

0:dc84eed6b8b6


   180

    setMode(1), 





narshu

0:dc84eed6b8b6


   181

   // sendCommand(cmdByte, 0x30);





narshu

0:dc84eed6b8b6


   182

    sendCommand(cmdSetMotor1, speed1);





narshu

0:dc84eed6b8b6


   183

    sendCommand(cmdSetMotor2, speed2);





narshu

0:dc84eed6b8b6


   184

}





narshu

0:dc84eed6b8b6


   185







narshu

0:dc84eed6b8b6


   186

void setMode(int mode) {





narshu

0:dc84eed6b8b6


   187

    sendCommand(cmdSetMode, mode);





narshu

0:dc84eed6b8b6


   188

}





narshu

0:dc84eed6b8b6


   189







narshu

0:dc84eed6b8b6


   190

long encoderToDistance(long encoder) {





narshu

0:dc84eed6b8b6


   191

    return long((float(encoder) / float(encoderRevCount)) * wheelmm);





narshu

0:dc84eed6b8b6


   192

}





narshu

0:dc84eed6b8b6


   193







narshu

0:dc84eed6b8b6


   194

long distanceToEncoder(long distance) {





narshu

0:dc84eed6b8b6


   195

    return long((float(distance) / float(wheelmm)) * encoderRevCount);





narshu

0:dc84eed6b8b6


   196

}





narshu

0:dc84eed6b8b6


   197







narshu

0:dc84eed6b8b6


   198

void sendCommand(char command) {





narshu

0:dc84eed6b8b6


   199

    char buffer[1];





narshu

0:dc84eed6b8b6


   200

    buffer[0] = command;





narshu

0:dc84eed6b8b6


   201

    i2c.write(md25Address, &buffer[0], 1);





narshu

0:dc84eed6b8b6


   202

}





narshu

0:dc84eed6b8b6


   203







narshu

0:dc84eed6b8b6


   204

void sendCommand(char command1, char command2 ) {





narshu

0:dc84eed6b8b6


   205







narshu

0:dc84eed6b8b6


   206

    char buffer[2];





narshu

0:dc84eed6b8b6


   207

    buffer[0] = command1;





narshu

0:dc84eed6b8b6


   208

    buffer[1] = command2;





narshu

0:dc84eed6b8b6


   209

    





narshu

0:dc84eed6b8b6


   210

    i2c.write(md25Address, &buffer[0], 2);





narshu

0:dc84eed6b8b6


   211

}





narshu

0:dc84eed6b8b6


   212







narshu

0:dc84eed6b8b6


   213







narshu

0:dc84eed6b8b6


   214