![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
test0417
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?
User | Revision | Line number | New 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 |