Hsieh JenYun / Mbed OS I2C_Master

a

Fork of I2C_Master by LAXAN01

main.cpp@6:46bb04ae559c, 2019-03-27 (annotated)

Committer:
Tanakacool
Date:
Wed Mar 27 05:27:59 2019 +0000
Revision:
6:46bb04ae559c
Parent:
5:221ce6ba655c 
V2

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 6:46bb04ae559c 11
Tanakacool 6:46bb04ae559c 12 DigitalOut LIFT_UP_PIN(D4);
Tanakacool 6:46bb04ae559c 13 DigitalOut LIFT_DOWN_PIN(D3);
Tanakacool 5:221ce6ba655c 14 //InterruptIn sw2(B1);
Tanakacool 5:221ce6ba655c 15 //I2C i2c( PTB1, PTB0);
Tanakacool 5:221ce6ba655c 16 I2C i2c( D14, D15);
Tanakacool 5:221ce6ba655c 17
Tanakacool 5:221ce6ba655c 18 Serial serialPort(USBTX, USBRX);
Tanakacool 5:221ce6ba655c 19 //Serial serialPort( PTE0, PTE1);
Tanakacool 2:3d64970eeb96 20
Tanakacool 4:186241a75818 21 char Rmotobuffer[3];
Tanakacool 4:186241a75818 22 char Lmotobuffer[3];
Tanakacool 3:996d0d74fb11 23
Tanakacool 2:3d64970eeb96 24 int Right_ADDR = 0x14<<1;
Tanakacool 3:996d0d74fb11 25 int Left_ADDR = 0x15<<1;
mbed_official 0:f76c26307f9a 26
Tanakacool 5:221ce6ba655c 27 int lin_vel1; //左輪
Tanakacool 5:221ce6ba655c 28 int lin_vel2; //右輪
Tanakacool 5:221ce6ba655c 29
Tanakacool 5:221ce6ba655c 30 int VELOCITY_CONSTANT_VAULE= 17000;
Tanakacool 5:221ce6ba655c 31 int LIMIT_X_MAX_VELOCITY = 8293; //max cmd:8293~=0.5m/s
Tanakacool 5:221ce6ba655c 32 int LIMIT_X_MIN_VELOCITY = 1640; //min value
Tanakacool 5:221ce6ba655c 33
Tanakacool 5:221ce6ba655c 34 /*
Tanakacool 2:3d64970eeb96 35 void sw2_release(void)
Tanakacool 2:3d64970eeb96 36 {
Tanakacool 2:3d64970eeb96 37 led_red = !led_red;
Tanakacool 2:3d64970eeb96 38 serialPort.printf("On-board button SW2 was released.\n");
Tanakacool 2:3d64970eeb96 39 }
Tanakacool 5:221ce6ba655c 40 */
Tanakacool 5:221ce6ba655c 41 void Driving_Right(int rightOutPut);
Tanakacool 5:221ce6ba655c 42 void Driving_Left(int leftOutPut);
Tanakacool 5:221ce6ba655c 43
Tanakacool 6:46bb04ae559c 44 void liftup(int stage);
Tanakacool 6:46bb04ae559c 45
Tanakacool 5:221ce6ba655c 46 void controlMotorSpeed(float goalLinearVel, float goalAngularVel);
Tanakacool 5:221ce6ba655c 47
Tanakacool 5:221ce6ba655c 48 int main()
Tanakacool 5:221ce6ba655c 49 {
Tanakacool 5:221ce6ba655c 50
Tanakacool 5:221ce6ba655c 51 //sw2.rise(&sw2_release);
Tanakacool 5:221ce6ba655c 52 serialPort.baud(9600);
Tanakacool 5:221ce6ba655c 53 serialPort.printf("WPI Serial Port Started\n");
Tanakacool 5:221ce6ba655c 54
Tanakacool 5:221ce6ba655c 55 i2c.frequency(100000);
Tanakacool 5:221ce6ba655c 56 wait_ms(100);
Tanakacool 5:221ce6ba655c 57
Tanakacool 5:221ce6ba655c 58 int motorOutPut=0;
Tanakacool 5:221ce6ba655c 59
Tanakacool 5:221ce6ba655c 60 Driving_Right(motorOutPut);
Tanakacool 5:221ce6ba655c 61 wait_ms(100);
Tanakacool 5:221ce6ba655c 62 Driving_Left(motorOutPut);
Tanakacool 6:46bb04ae559c 63
Tanakacool 6:46bb04ae559c 64 liftup(0);
Tanakacool 5:221ce6ba655c 65
Tanakacool 5:221ce6ba655c 66 while (true) {
Tanakacool 5:221ce6ba655c 67
Tanakacool 5:221ce6ba655c 68
Tanakacool 5:221ce6ba655c 69
Tanakacool 5:221ce6ba655c 70 }
Tanakacool 5:221ce6ba655c 71 }
Tanakacool 5:221ce6ba655c 72
Tanakacool 5:221ce6ba655c 73 void controlMotorSpeed(float goalLinearVel, float goalAngularVel)
Tanakacool 2:3d64970eeb96 74 {
Tanakacool 5:221ce6ba655c 75 float wheel_speed_cmd[2];
Tanakacool 5:221ce6ba655c 76
Tanakacool 5:221ce6ba655c 77 wheel_speed_cmd[LEFT] = goalLinearVel - (goalAngularVel * WHEEL_SEPARATION / 2);
Tanakacool 5:221ce6ba655c 78 wheel_speed_cmd[RIGHT] = goalLinearVel + (goalAngularVel * WHEEL_SEPARATION / 2);
Tanakacool 5:221ce6ba655c 79
Tanakacool 5:221ce6ba655c 80 lin_vel1 = wheel_speed_cmd[LEFT] * VELOCITY_CONSTANT_VAULE;
Tanakacool 5:221ce6ba655c 81 wait_ms(2);
Tanakacool 5:221ce6ba655c 82
Tanakacool 5:221ce6ba655c 83 if (lin_vel1 > LIMIT_X_MAX_VELOCITY) lin_vel1 = LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 84 else if (lin_vel1 < LIMIT_X_MIN_VELOCITY && lin_vel1 > 0 ) lin_vel1 = LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 85
Tanakacool 5:221ce6ba655c 86 else if (lin_vel1 < -LIMIT_X_MAX_VELOCITY) lin_vel1 = -LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 87 else if (lin_vel1 > -LIMIT_X_MIN_VELOCITY && lin_vel1 < 0) lin_vel1 = -LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 88
Tanakacool 5:221ce6ba655c 89 lin_vel2 = wheel_speed_cmd[RIGHT] * VELOCITY_CONSTANT_VAULE;
Tanakacool 5:221ce6ba655c 90 wait_ms(2);
Tanakacool 5:221ce6ba655c 91
Tanakacool 5:221ce6ba655c 92
Tanakacool 5:221ce6ba655c 93 if (lin_vel2 > LIMIT_X_MAX_VELOCITY) lin_vel2 = LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 94 else if (lin_vel2 < LIMIT_X_MIN_VELOCITY && lin_vel2 > 0 ) lin_vel2 = LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 95
Tanakacool 5:221ce6ba655c 96 else if (lin_vel2 < -LIMIT_X_MAX_VELOCITY) lin_vel2 = -LIMIT_X_MAX_VELOCITY;
Tanakacool 5:221ce6ba655c 97 else if (lin_vel2 > -LIMIT_X_MIN_VELOCITY && lin_vel2 < 0) lin_vel2 = -LIMIT_X_MIN_VELOCITY;
Tanakacool 5:221ce6ba655c 98
Tanakacool 5:221ce6ba655c 99 }
Tanakacool 6:46bb04ae559c 100 void liftup(int stage)
Tanakacool 6:46bb04ae559c 101 {
Tanakacool 6:46bb04ae559c 102 switch (stage) {
Tanakacool 6:46bb04ae559c 103 case 0: //stop
Tanakacool 6:46bb04ae559c 104 LIFT_UP_PIN=0;
Tanakacool 6:46bb04ae559c 105 LIFT_DOWN_PIN=0;
Tanakacool 5:221ce6ba655c 106
Tanakacool 6:46bb04ae559c 107 break;
Tanakacool 6:46bb04ae559c 108 case 1: //up
Tanakacool 6:46bb04ae559c 109 LIFT_UP_PIN=1;
Tanakacool 6:46bb04ae559c 110 LIFT_DOWN_PIN=0;
Tanakacool 6:46bb04ae559c 111
Tanakacool 6:46bb04ae559c 112 break;
Tanakacool 6:46bb04ae559c 113 case 2: //down
Tanakacool 6:46bb04ae559c 114 LIFT_UP_PIN=0;
Tanakacool 6:46bb04ae559c 115 LIFT_DOWN_PIN=1;
Tanakacool 6:46bb04ae559c 116
Tanakacool 6:46bb04ae559c 117 break;
Tanakacool 6:46bb04ae559c 118
Tanakacool 6:46bb04ae559c 119 }
Tanakacool 6:46bb04ae559c 120
Tanakacool 6:46bb04ae559c 121 }
Tanakacool 5:221ce6ba655c 122 void Driving_Right(int rightOutPut)
Tanakacool 5:221ce6ba655c 123 {
Tanakacool 5:221ce6ba655c 124 Rmotobuffer[0]= 0x00;
Tanakacool 5:221ce6ba655c 125 Rmotobuffer[1]= rightOutPut>>8;
Tanakacool 5:221ce6ba655c 126 Rmotobuffer[2]= rightOutPut;
Tanakacool 2:3d64970eeb96 127 i2c.start();
Tanakacool 2:3d64970eeb96 128 i2c.write(Right_ADDR);
Tanakacool 2:3d64970eeb96 129 i2c.write(0);
Tanakacool 4:186241a75818 130 i2c.write(Rmotobuffer[1]);
Tanakacool 4:186241a75818 131 i2c.write(Rmotobuffer[2]);
Tanakacool 2:3d64970eeb96 132 i2c.stop();
Tanakacool 2:3d64970eeb96 133 }
Tanakacool 2:3d64970eeb96 134
Tanakacool 5:221ce6ba655c 135 void Driving_Left(int leftOutPut)
Tanakacool 2:3d64970eeb96 136 {
Tanakacool 5:221ce6ba655c 137 Lmotobuffer[0]= 0x00;
Tanakacool 5:221ce6ba655c 138 Lmotobuffer[1]= leftOutPut>>8;
Tanakacool 5:221ce6ba655c 139 Lmotobuffer[2]= leftOutPut;
Tanakacool 5:221ce6ba655c 140 i2c.start();
Tanakacool 5:221ce6ba655c 141 i2c.write(Left_ADDR);
Tanakacool 5:221ce6ba655c 142 i2c.write(0);
Tanakacool 5:221ce6ba655c 143 i2c.write(Lmotobuffer[1]);
Tanakacool 5:221ce6ba655c 144 i2c.write(Lmotobuffer[2]);
Tanakacool 5:221ce6ba655c 145 i2c.stop();
Tanakacool 6:46bb04ae559c 146
Tanakacool 5:221ce6ba655c 147 }
Tanakacool 3:996d0d74fb11 148