Revision 103:b417a6c65a6f, committed 2019-06-07

Comitter:

Jeonghoon

Date:

Fri Jun 07 17:10:18 2019 +0000

Parent:

102:e846a127af54

Child:

104:94ed54c74466

Commit message:

20190608 2:09am stable ver.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PixelArray.lib	Fri Jun 07 17:10:18 2019 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/chris/code/PixelArray/#b45a70faaa83

--- a/RemoteIR.lib	Fri Jun 07 14:50:44 2019 +0000
+++ b/RemoteIR.lib	Fri Jun 07 17:10:18 2019 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/embedded1/code/RemoteIR/#3922b10d4606
+https://os.mbed.com/teams/embedded1/code/RemoteIR/#6d2532146627

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WS2812.lib	Fri Jun 07 17:10:18 2019 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/bridadan/code/WS2812/#6e647820f587

--- a/main.cpp	Fri Jun 07 14:50:44 2019 +0000
+++ b/main.cpp	Fri Jun 07 17:10:18 2019 +0000
@@ -8,17 +8,16 @@
 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);
-RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
+Serial 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);
-
-RawSerial blue(PA_11, PA_12, 9600);
+ 
+Serial 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;
@@ -41,13 +40,7 @@
 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);
@@ -61,16 +54,9 @@
 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);
@@ -95,133 +81,6 @@
  
     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){
  
@@ -246,10 +105,10 @@
     motor.cal_start = 0;
     motor.cal_stop = 0; 
     
-    kp = 10;
-    kd = 1;
-    ki = 10000;
-    max_speed = 100;
+    motor.kp = 10;
+    motor.kd = 1;
+    motor.ki = 5000;
+    motor.max = 150;
  
     DataComm = 0;
  
@@ -268,7 +127,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", kp, kd, ki, max_speed);
+        myOled.printf("%.0f %.1f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
         myOled.display();
     }
 }
@@ -384,6 +243,12 @@
         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(){
@@ -433,36 +298,31 @@
  
     last_proportional  = proportional;
  
-    int power_difference = proportional / kp + integral / ki + derivative * kd;
+    int power_difference = proportional / motor.kp + integral / motor.ki + derivative * motor.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 > max_speed)
-        power_difference = max_speed;
+    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;
     
     // bot position: right
     if(power_difference > 15){ //0.1 + p_d/std
-        motor.speed_r((max_speed + power_difference)/std_rate);
-        motor.speed_l((max_speed - power_difference)/std_rate);
+        motor.speed_r((motor.max + power_difference)/std);
+        motor.speed_l((motor.max - power_difference)/std);
     }
     // bot position: left
     else if(power_difference < -15){
-        motor.speed_r((max_speed + power_difference)/std_rate);
-        motor.speed_l((max_speed - power_difference)/std_rate);
+        motor.speed_r((motor.max + power_difference)/std);
+        motor.speed_l((motor.max - power_difference)/std);
     }
     else{ //online
-        motor.speed_l(init_sp_l);
-        motor.speed_r(init_sp_r);
+        motor.speed_l(motor.init_sp_l);
+        motor.speed_r(motor.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