Diff: main.cpp

Revision:

5:221ce6ba655c

Parent:

4:186241a75818

Child:

6:46bb04ae559c

--- a/main.cpp	Wed Oct 24 06:16:00 2018 +0000
+++ b/main.cpp	Tue Mar 26 01:16:55 2019 +0000
@@ -1,10 +1,19 @@
 #include "mbed.h"
 
-DigitalOut led_red(LED_RED);
-InterruptIn sw2(SW2);
-I2C i2c( PTB1, PTB0);
+#define LEFT 0
+#define RIGHT 1
+
+#define WHEEL_SEPARATION 4
+
+//DigitalOut led_red(LED_RED);
 
-Serial serialPort( PTE0, PTE1);
+DigitalOut led1(LED1);
+//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];
@@ -12,53 +21,99 @@
 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(void)
+*/
+void Driving_Right(int rightOutPut);
+void Driving_Left(int leftOutPut);
+
+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);
+
+    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 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(Lmotobuffer[1]);
-    i2c.write(Lmotobuffer[2]);
-    i2c.stop();
-}
-void Driving_Left(void)
-{
-    i2c.start();
-    i2c.write(Left_ADDR);
-    i2c.write(0);
     i2c.write(Rmotobuffer[1]);
     i2c.write(Rmotobuffer[2]);
     i2c.stop();
 }
 
-int main()
+void Driving_Left(int leftOutPut)
 {
-
-    sw2.rise(&sw2_release);
-    serialPort.baud(115200);
-    serialPort.printf("WPI Serial Port Started\n");
-
-    i2c.frequency(100000);
-    wait_ms(100);
+    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();
+}
 
-    int motorOutPut=2000;
-    Rmotobuffer[0]=0x00;
-    Rmotobuffer[1]= motorOutPut>>8;
-    Rmotobuffer[2]=motorOutPut;
-    Lmotobuffer[0]=0x00;
-    Lmotobuffer[1]= motorOutPut>>8;
-    Lmotobuffer[2]=motorOutPut;
-
-    Driving_Right();
-    wait_ms(100);
-    Driving_Left();
-
-    while (true) {
-
-    }
-}