File content as of revision 7:46bb04ae559c:

#include "mbed.h"

#define LEFT 0
#define RIGHT 1

#define WHEEL_SEPARATION 4

//DigitalOut led_red(LED_RED);

DigitalOut led1(LED1);

DigitalOut LIFT_UP_PIN(D4);
DigitalOut LIFT_DOWN_PIN(D3);
//InterruptIn sw2(B1);
//I2C i2c( PTB1, PTB0);
I2C i2c( D14, D15);

Serial serialPort(USBTX, USBRX);
//Serial serialPort( PTE0, PTE1);

char Rmotobuffer[3];
char Lmotobuffer[3];

int Right_ADDR = 0x14<<1;
int Left_ADDR  = 0x15<<1;

int lin_vel1;  //左輪
int lin_vel2;  //右輪

int VELOCITY_CONSTANT_VAULE= 17000;
int LIMIT_X_MAX_VELOCITY = 8293;      //max cmd:8293~=0.5m/s
int LIMIT_X_MIN_VELOCITY = 1640;       //min value

/*
void sw2_release(void)
{
    led_red = !led_red;
    serialPort.printf("On-board button SW2 was released.\n");
}
*/
void Driving_Right(int rightOutPut);
void Driving_Left(int leftOutPut);

void liftup(int stage);

void controlMotorSpeed(float goalLinearVel, float goalAngularVel);

int main()
{

    //sw2.rise(&sw2_release);
    serialPort.baud(9600);
    serialPort.printf("WPI Serial Port Started\n");

    i2c.frequency(100000);
    wait_ms(100);

    int motorOutPut=0;

    Driving_Right(motorOutPut);
    wait_ms(100);
    Driving_Left(motorOutPut);
    
    liftup(0);

    while (true) {



    }
}

void controlMotorSpeed(float goalLinearVel, float goalAngularVel)
{
    float wheel_speed_cmd[2];

    wheel_speed_cmd[LEFT]  = goalLinearVel - (goalAngularVel * WHEEL_SEPARATION / 2);
    wheel_speed_cmd[RIGHT] = goalLinearVel + (goalAngularVel * WHEEL_SEPARATION / 2);

    lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE;
    wait_ms(2);

    if (lin_vel1 > LIMIT_X_MAX_VELOCITY)       lin_vel1 =  LIMIT_X_MAX_VELOCITY;
    else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY;

    else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY;
    else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY;

    lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE;
    wait_ms(2);


    if (lin_vel2 > LIMIT_X_MAX_VELOCITY)       lin_vel2 =  LIMIT_X_MAX_VELOCITY;
    else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY;

    else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY;
    else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY;

}
void liftup(int stage)
{
    switch (stage) {
        case 0:         //stop
            LIFT_UP_PIN=0;
            LIFT_DOWN_PIN=0;

            break;
        case 1:         //up
            LIFT_UP_PIN=1;
            LIFT_DOWN_PIN=0;

            break;
        case 2:         //down
            LIFT_UP_PIN=0;
            LIFT_DOWN_PIN=1;

            break;

    }

}
void Driving_Right(int rightOutPut)
{
    Rmotobuffer[0]= 0x00;
    Rmotobuffer[1]= rightOutPut>>8;
    Rmotobuffer[2]= rightOutPut;
    i2c.start();
    i2c.write(Right_ADDR);
    i2c.write(0);
    i2c.write(Rmotobuffer[1]);
    i2c.write(Rmotobuffer[2]);
    i2c.stop();
}

void Driving_Left(int leftOutPut)
{
    Lmotobuffer[0]= 0x00;
    Lmotobuffer[1]= leftOutPut>>8;
    Lmotobuffer[2]= leftOutPut;
    i2c.start();
    i2c.write(Left_ADDR);
    i2c.write(0);
    i2c.write(Lmotobuffer[1]);
    i2c.write(Lmotobuffer[2]);
    i2c.stop();

}