Hsieh JenYun / Mbed OS agv20190417

test0417

Dependencies:   ros_lib_indigo

main.cpp@5:221ce6ba655c, 2019-03-26 (annotated)

Committer:
Tanakacool
Date:
Tue Mar 26 01:16:55 2019 +0000
Revision:
5:221ce6ba655c
Parent:
4:186241a75818
Child:
6:4e03a22f2abb
Child:
7:46bb04ae559c
TEST

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:f76c26307f9a 1 #include "mbed.h"
cathy101 1:6926cb0de2b0 2
Tanakacool 5:221ce6ba655c 3 #define LEFT 0
Tanakacool 5:221ce6ba655c 4 #define RIGHT 1
Tanakacool 5:221ce6ba655c 5
Tanakacool 5:221ce6ba655c 6 #define WHEEL_SEPARATION 4
Tanakacool 5:221ce6ba655c 7
Tanakacool 5:221ce6ba655c 8 //DigitalOut led_red(LED_RED);
mbed_official 0:f76c26307f9a 9
Tanakacool 5:221ce6ba655c 10 DigitalOut led1(LED1);
Tanakacool 5:221ce6ba655c 11 //InterruptIn sw2(B1);
Tanakacool 5:221ce6ba655c 12 //I2C i2c( PTB1, PTB0);
Tanakacool 5:221ce6ba655c 13 I2C i2c( D14, D15);
Tanakacool 5:221ce6ba655c 14
Tanakacool 5:221ce6ba655c 15 Serial serialPort(USBTX, USBRX);
Tanakacool 5:221ce6ba655c 16 //Serial serialPort( PTE0, PTE1);
Tanakacool 2:3d64970eeb96 17
Tanakacool 4:186241a75818 18 char Rmotobuffer[3];
Tanakacool 4:186241a75818 19 char Lmotobuffer[3];
Tanakacool 3:996d0d74fb11 20
Tanakacool 2:3d64970eeb96 21 int Right_ADDR = 0x14<<1;
Tanakacool 3:996d0d74fb11 22 int Left_ADDR = 0x15<<1;
mbed_official 0:f76c26307f9a 23
Tanakacool 5:221ce6ba655c 24 int lin_vel1; //左輪
Tanakacool 5:221ce6ba655c 25 int lin_vel2; //右輪
Tanakacool 5:221ce6ba655c 26
Tanakacool 5:221ce6ba655c 27 int VELOCITY_CONSTANT_VAULE= 17000;
Tanakacool 5:221ce6ba655c 28 int LIMIT_X_MAX_VELOCITY = 8293; //max cmd:8293~=0.5m/s
Tanakacool 5:221ce6ba655c 29 int LIMIT_X_MIN_VELOCITY = 1640; //min value
Tanakacool 5:221ce6ba655c 30
Tanakacool 5:221ce6ba655c 31 /*
Tanakacool 2:3d64970eeb96 32 void sw2_release(void)
Tanakacool 2:3d64970eeb96 33 {
Tanakacool 2:3d64970eeb96 34 led_red = !led_red;
Tanakacool 2:3d64970eeb96 35 serialPort.printf("On-board button SW2 was released.\n");
Tanakacool 2:3d64970eeb96 36 }
Tanakacool 5:221ce6ba655c 37 */
Tanakacool 5:221ce6ba655c 38 void Driving_Right(int rightOutPut);
Tanakacool 5:221ce6ba655c 39 void Driving_Left(int leftOutPut);
Tanakacool 5:221ce6ba655c 40
Tanakacool 5:221ce6ba655c 41 void controlMotorSpeed(float goalLinearVel, float goalAngularVel);
Tanakacool 5:221ce6ba655c 42
Tanakacool 5:221ce6ba655c 43 int main()
Tanakacool 5:221ce6ba655c 44 {
Tanakacool 5:221ce6ba655c 45
Tanakacool 5:221ce6ba655c 46 //sw2.rise(&sw2_release);
Tanakacool 5:221ce6ba655c 47 serialPort.baud(9600);
Tanakacool 5:221ce6ba655c 48 serialPort.printf("WPI Serial Port Started\n");
Tanakacool 5:221ce6ba655c 49
Tanakacool 5:221ce6ba655c 50 i2c.frequency(100000);
Tanakacool 5:221ce6ba655c 51 wait_ms(100);
Tanakacool 5:221ce6ba655c 52
Tanakacool 5:221ce6ba655c 53 int motorOutPut=0;
Tanakacool 5:221ce6ba655c 54
Tanakacool 5:221ce6ba655c 55 Driving_Right(motorOutPut);
Tanakacool 5:221ce6ba655c 56 wait_ms(100);
Tanakacool 5:221ce6ba655c 57 Driving_Left(motorOutPut);
Tanakacool 5:221ce6ba655c 58
Tanakacool 5:221ce6ba655c 59 while (true) {
Tanakacool 5:221ce6ba655c 60
Tanakacool 5:221ce6ba655c 61
Tanakacool 5:221ce6ba655c 62
Tanakacool 5:221ce6ba655c 63 }
Tanakacool 5:221ce6ba655c 64 }
Tanakacool 5:221ce6ba655c 65
Tanakacool 5:221ce6ba655c 66 void controlMotorSpeed(float goalLinearVel, float goalAngularVel)
Tanakacool 2:3d64970eeb96 67 {
Tanakacool 5:221ce6ba655c 68 float wheel_speed_cmd[2];
Tanakacool 5:221ce6ba655c 69
Tanakacool 5:221ce6ba655c 70 wheel_speed_cmd[LEFT] = goalLinearVel - (goalAngularVel * WHEEL_SEPARATION / 2);
Tanakacool 5:221ce6ba655c 71 wheel_speed_cmd[RIGHT] = goalLinearVel + (goalAngularVel * WHEEL_SEPARATION / 2);
Tanakacool 5:221ce6ba655c 72
Tanakacool 5:221ce6ba655c 73 lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE;
Tanakacool 5:221ce6ba655c 74 wait_ms(2);
Tanakacool 5:221ce6ba655c 75
Tanakacool 5:221ce6ba655c 76 if (lin_vel1 > LIMIT_X_MAX_VELOCITY) lin_vel1 = LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 77 else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 78
Tanakacool 5:221ce6ba655c 79 else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 80 else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 81
Tanakacool 5:221ce6ba655c 82 lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE;
Tanakacool 5:221ce6ba655c 83 wait_ms(2);
Tanakacool 5:221ce6ba655c 84
Tanakacool 5:221ce6ba655c 85
Tanakacool 5:221ce6ba655c 86 if (lin_vel2 > LIMIT_X_MAX_VELOCITY) lin_vel2 = LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 87 else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 88
Tanakacool 5:221ce6ba655c 89 else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 90 else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 91
Tanakacool 5:221ce6ba655c 92 }
Tanakacool 5:221ce6ba655c 93
Tanakacool 5:221ce6ba655c 94 void Driving_Right(int rightOutPut)
Tanakacool 5:221ce6ba655c 95 {
Tanakacool 5:221ce6ba655c 96 Rmotobuffer[0]= 0x00;
Tanakacool 5:221ce6ba655c 97 Rmotobuffer[1]= rightOutPut>>8;
Tanakacool 5:221ce6ba655c 98 Rmotobuffer[2]= rightOutPut;
Tanakacool 2:3d64970eeb96 99 i2c.start();
Tanakacool 2:3d64970eeb96 100 i2c.write(Right_ADDR);
Tanakacool 2:3d64970eeb96 101 i2c.write(0);
Tanakacool 4:186241a75818 102 i2c.write(Rmotobuffer[1]);
Tanakacool 4:186241a75818 103 i2c.write(Rmotobuffer[2]);
Tanakacool 2:3d64970eeb96 104 i2c.stop();
Tanakacool 2:3d64970eeb96 105 }
Tanakacool 2:3d64970eeb96 106
Tanakacool 5:221ce6ba655c 107 void Driving_Left(int leftOutPut)
Tanakacool 2:3d64970eeb96 108 {
Tanakacool 5:221ce6ba655c 109 Lmotobuffer[0]= 0x00;
Tanakacool 5:221ce6ba655c 110 Lmotobuffer[1]= leftOutPut>>8;
Tanakacool 5:221ce6ba655c 111 Lmotobuffer[2]= leftOutPut;
Tanakacool 5:221ce6ba655c 112 i2c.start();
Tanakacool 5:221ce6ba655c 113 i2c.write(Left_ADDR);
Tanakacool 5:221ce6ba655c 114 i2c.write(0);
Tanakacool 5:221ce6ba655c 115 i2c.write(Lmotobuffer[1]);
Tanakacool 5:221ce6ba655c 116 i2c.write(Lmotobuffer[2]);
Tanakacool 5:221ce6ba655c 117 i2c.stop();
Tanakacool 5:221ce6ba655c 118 }
Tanakacool 3:996d0d74fb11 119