Revision 102:e846a127af54, committed 2019-06-07

Comitter:

Jeonghoon

Date:

Fri Jun 07 14:50:44 2019 +0000

Parent:

101:efa2315d0312

Child:

103:b417a6c65a6f

Commit message:

20190607 bluetooth

Changed in this revision

--- a/RemoteIR.lib	Thu Jun 06 11:16:43 2019 +0000
+++ b/RemoteIR.lib	Fri Jun 07 14:50:44 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/embedded1/code/RemoteIR/#b56a97811380
+https://os.mbed.com/teams/embedded1/code/RemoteIR/#3922b10d4606

--- a/main.cpp	Thu Jun 06 11:16:43 2019 +0000
+++ b/main.cpp	Fri Jun 07 14:50:44 2019 +0000
@@ -8,16 +8,17 @@
 ReceiverIR motor(D4, 0.2, D6, D5, PA_0, PA_1, PB_0, PA_4);
 SPI spi(D11, D12, D13);
 DigitalOut spi_cs(D10, 1);
-Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
 I2C i2c(D14, D15);                                      // D14, D15
 Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128);    // D9, (D8: D/C - data/command change)
 DigitalOut DataComm(D8);
 
-Serial blue(PA_11, PA_12, 9600);
+RawSerial blue(PA_11, PA_12, 9600);
  
 Thread TR_thread;
 Thread remote_thread;
 Thread oled_thread;
+Thread blue_thread;
  
 unsigned long avg = 0;
 unsigned int sum = 0;
@@ -40,7 +41,13 @@
 volatile int start = 0;
  
 int ch;
- 
+
+float init_sp_r, init_sp_l;
+float kp, kd;
+float std_rate;
+int ki;
+int max_speed;
+int index = 0;
  
  
 int readLine(unsigned int *sensor_values);
@@ -54,9 +61,16 @@
 int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
 void remote_ctrl(void);
 void oled_disp(void);
+
+void pc_rx_cb(void);
+void blue_rx_cb(void);
  
 int main()
 {
+    pc.attach(&pc_rx_cb);
+    blue.attach(&blue_rx_cb);
+    blue_thread.start(&blue_rx_cb);
+            
     pc.printf("start\r\n");
     spi.format(16, 0);
     spi.frequency(2000000);
@@ -81,6 +95,133 @@
  
     while (1);
 }
+
+void pc_rx_cb(void){            // RX ISR(interrupt service routine)
+    char pc_ch;    
+    
+    pc_ch = pc.getc();
+    pc.putc(pc_ch);             //echo
+    blue.putc(pc_ch);
+    
+    if(pc_ch == 0x0D){          // CR(carriage return)
+        pc.putc(0x0A);          // LF(line feed
+        blue.putc(0x0A);
+    }
+}
+
+void blue_rx_cb(void){           // RX ISR(interrupt service routine)
+    while(1){
+    char buffer[50];
+    char blue_ch = blue.getc();
+    blue.putc(blue_ch);          //echo
+    pc.putc(blue_ch);
+
+    buffer[index++] = blue_ch;
+    
+    if(blue_ch == 0x0D){
+        pc.putc(0x0A);
+        blue.putc(0x0A);
+        
+        buffer[--index] = '\0';
+        
+        char* tok1 = strtok(buffer, " ");
+
+        // pid
+        if(!strncmp(tok1, "kp", 2)){
+            tok1 = strtok(NULL, " ");
+            kp = atof(tok1);
+            blue.printf("kp = %.2f\r\n", kp);
+        }
+        else if(!strncmp(tok1, "ki", 2)){
+            tok1 = strtok(NULL, " ");
+            ki = atoi(tok1);
+            blue.printf("ki = %d\r\n", ki);
+        }
+        else if(!strncmp(tok1, "kd", 2)){
+            tok1 = strtok(NULL, " ");
+            kd = atof(tok1);
+            blue.printf("kd = %.2f\r\n", kd);        
+        }
+        else if(!strncmp(tok1, "max", 3)){
+            tok1 = strtok(NULL, " ");
+            max_speed = atoi(tok1);
+            blue.printf("max = %d\r\n", max_speed);
+        }
+        else if(!strncmp(tok1, "std", 3)){
+            tok1 = strtok(NULL, " ");
+            std_rate = atof(tok1);
+            blue.printf("std = %.f\r\n", std_rate);
+        }        
+        // calibration
+        else if(!strncmp(buffer, "cal", 3)){
+            motor.cal_start = 1;
+            blue.printf("calibration start\r\n");
+        }
+        else if(!strncmp(buffer, "race", 4)){
+            motor.cal_stop = 1;
+            blue.printf("race start\r\n");
+        }
+        
+        // set speed
+        else if(!strncmp(tok1, "spl", 3)){
+            tok1 = strtok(NULL, " ");
+            motor.speed_l(atof(tok1));
+            blue.printf("motor.Speed_L = %.3f\r\n", motor.Speed_L);         
+        }
+        else if(!strncmp(buffer, "spr", 3)){
+            tok1 = strtok(NULL, " ");
+            motor.speed_r(atof(tok1));
+            blue.printf("motor.Speed_R = %.3f\r\n", motor.Speed_R);
+        }
+        
+        // stop
+        else if(!strncmp(buffer, "stop", 4)){
+            motor.stop(); 
+            blue.printf("alphabot stop\r\n");
+        }
+        // forward
+        else if(!strncmp(buffer, "fw", 2)){
+            motor.forward();
+            blue.printf("forward\r\n");
+        }
+        // backward
+        else if(!strncmp(buffer, "bw", 2)){
+            motor.backward();
+            blue.printf("backward\r\n");            
+        }
+        // left
+        else if(!strncmp(buffer, "left", 4)){
+            motor.left();
+            blue.printf("left\r\n");
+        }
+        // right
+        else if(!strncmp(buffer, "right", 5)){
+            motor.right();
+            blue.printf("right\r\n");               
+        }
+
+        // init speed
+        else if(!strncmp(tok1, "ispl", 4)){
+            tok1 = strtok(NULL, " ");
+            init_sp_l = atof(tok1);
+
+            blue.printf("init_sp_l: %.3f\r\n", init_sp_l);
+        }
+        else if(!strncmp(tok1, "ispr", 4)){
+            tok1 = strtok(NULL, " ");
+            init_sp_r = atof(tok1);
+
+            blue.printf("init_sp_r: %.3f\r\n", init_sp_r);
+        }       
+        else{
+
+            blue.printf("undefined command\r\n");
+        }
+        
+        index = -1;
+    }
+    }
+}
  
 void remote_ctrl(void){
  
@@ -105,10 +246,10 @@
     motor.cal_start = 0;
     motor.cal_stop = 0; 
     
-    motor.kp = 10;
-    motor.kd = 1;
-    motor.ki = 5000;
-    motor.max = 150;
+    kp = 10;
+    kd = 1;
+    ki = 10000;
+    max_speed = 100;
  
     DataComm = 0;
  
@@ -127,7 +268,7 @@
     while(1){
         myOled.clearDisplay();
         //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
-        myOled.printf("%.0f %.1f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
+        myOled.printf("%.0f %.1f %d %d\r", kp, kd, ki, max_speed);
         myOled.display();
     }
 }
@@ -243,12 +384,6 @@
         wait(5);        
     }
         
-   for(int i = 0; i < NUM_SENSORS; i++){
- 
-        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
-    }
-    
-    pc.printf("\r\n");
 }
  
 void get_pos(){
@@ -298,34 +433,36 @@
  
     last_proportional  = proportional;
  
-    int power_difference = proportional / motor.kp + integral / motor.ki + derivative * motor.kd;
+    int power_difference = proportional / kp + integral / ki + derivative * kd;
     //int power_difference = proportional / motor.kp;
     //int power_difference = proportional/motor.kp + derivative * motor.kd; 
-    const float std = 600.0;
+    //const float std = 600.0;
  
-    if(power_difference > motor.max)
-        power_difference = motor.max;
+    if(power_difference > max_speed)
+        power_difference = max_speed;
  
-    if(power_difference < -motor.max)
-        power_difference = -motor.max;
+    if(power_difference < -max_speed)
+        power_difference = -max_speed;
     
     // bot position: right
     if(power_difference > 15){ //0.1 + p_d/std
-        motor.speed_r((motor.max + power_difference)/std);
-        motor.speed_l((motor.max - power_difference)/std);
+        motor.speed_r((max_speed + power_difference)/std_rate);
+        motor.speed_l((max_speed - power_difference)/std_rate);
     }
     // bot position: left
     else if(power_difference < -15){
-        motor.speed_r((motor.max + power_difference)/std);
-        motor.speed_l((motor.max - power_difference)/std);
+        motor.speed_r((max_speed + power_difference)/std_rate);
+        motor.speed_l((max_speed - power_difference)/std_rate);
     }
     else{ //online
-        motor.speed_l(motor.init_sp_l);
-        motor.speed_r(motor.init_sp_r);
+        motor.speed_l(init_sp_l);
+        motor.speed_r(init_sp_r);
     }
  
     // pc.printf("proportional: %d\r\n", proportional);
      //pc.printf("(max + power_difference): %d\r\n", motor.max + power_difference);
     // pc.printf("derivative: %d\r\n", derivative);
     // pc.printf("integral: %d\r\n",integral);
-}
\ No newline at end of file
+}
+
+