Diff: main.cpp

Revision:

100:0e44e944a19f

Parent:

97:bad45e2dc7e1

Child:

101:efa2315d0312

diff -r ac298f47375b -r 0e44e944a19f main.cpp
--- a/main.cpp	Wed Jun 05 04:55:45 2019 +0000
+++ b/main.cpp	Wed Jun 05 18:40:06 2019 +0000
@@ -1,344 +1,328 @@
-#include "mbed.h"
-//#include "motor.h"
-#include "ReceiverIR.h"
-#include "Adafruit_SSD1306.h"   // OLED
-#define NUM_SENSORS 5
-
-//Motor motor(D6, D5, PA_0, PA_1, PB_0, PA_4, 0.3);
-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);
-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);
-
-Thread TR_thread;
-Thread remote_thread;
-Thread oled_thread;
-
-unsigned long avg = 0;
-unsigned int sum = 0;
-
-unsigned int sensor_values[NUM_SENSORS];
-unsigned int max_sensor_values[NUM_SENSORS];
-unsigned int min_sensor_values[NUM_SENSORS]; 
-
-unsigned int *calibratedMin;
-unsigned int *calibratedMax;   
-
-int value;
-float bat;
-int on_line = 0;
-static int pos = 0;
-unsigned int last_proportional = 0;
-long integral = 0;
-
-volatile int flag = 0;
-volatile int start = 0;
-
-int ch;
-
-int readLine(unsigned int *sensor_values);
-void tr_ready(void);
-void calibration(void);
-void init(void);
-void tracing(void);
-//void tracing_pid(void);
-void set_ir_val(void);
-void get_pos(void);
-int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
-void remote_ctrl(void);
-void oled_disp(void);
-
-int main()
-{   
-    pc.printf("start\r\n");
-    spi.format(16, 0);
-    spi.frequency(2000000);
-
-    init();
-    
-    oled_thread.start(&oled_disp);
-    remote_thread.start(&remote_ctrl);    
-    while(!motor.start);
-    
-    calibration();
-    
-    while(!flag);
-
-    TR_thread.start(&tr_ready);
-
-    
-    while(!start);
-      
-    pc.printf("motor forward\r\n");  
-    motor.forward();
-    
-    while (1);
-}
-
-void remote_ctrl(void){
-    
-    while(1){
-    uint8_t buf1[32];
-    uint8_t buf2[32];
-    int bitlength1;
-    RemoteIR::Format format;
-
-    memset(buf1, 0x00, sizeof(buf1));
-    memset(buf2, 0x00, sizeof(buf2));
-
-        bitlength1 = receive(&format, buf1, sizeof(buf1));
-        if (bitlength1 < 0) {
-            continue;
-        }
-        
-        //display_status("RECV", bitlength1);
-        //display_data(buf1, bitlength1);
-        //display_format(format);  
-
-    }  
-}
-
-void init(void){
-    DataComm = 0;
-    
-    calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
-    calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
-
-    for(int i=0; i < NUM_SENSORS; i++)
-    {
-      calibratedMin[i] = 1023;
-      calibratedMax[i] = 0;
-    }
-}
-
-void oled_disp(void){
-    myOled.begin();
-    while(1){
-        myOled.clearDisplay();
-        myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
-        myOled.display();          
-    }
-}
-
-int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
-    int cnt = 0;
-    while (motor.getState() != ReceiverIR::Received) {
-        cnt++;
-        if (timeout < cnt) {
-            return -1;
-        }
-    }
-    return motor.getData(format, buf, bufsiz * 8);
-}
-
-
-void read_ir(void){
-    ch = 0;
-        
-    spi_cs = 0;
-    wait_us(2);
-    value = spi.write(ch << 12);
-    spi_cs = 1;
-        
-    wait_us(21);                     
-    
-    for(int i = 0; i < 5; i++){
-        
-        ch += 1;
-        
-        spi_cs = 0;
-        wait_us(2);
-        value = spi.write(ch << 12);
-        spi_cs = 1;
-            
-        wait_us(21);                   
-    
-        value = value >> 6;
-        value = value * 3300 / 0x3FF;
-        
-        sensor_values[i] = value;
-    }
-}
-
-void calibration(void){
-    pc.printf("calibration start \r\n");
-    
-    for(int i = 0; i < 10; i++){
-        read_ir();
-        for(int j = 0; j < NUM_SENSORS; j++){
-            if(i == 0 || max_sensor_values[j] < sensor_values[j]){
-                max_sensor_values[j] = sensor_values[j];
-            }
-    
-            if(i == 0 || min_sensor_values[j] > sensor_values[j]){
-                min_sensor_values[j] = sensor_values[j];
-            }
-        }
-        wait(0.5);
-    }
-    
-    for(int i = 0; i < NUM_SENSORS; i++){
-        if(min_sensor_values[i] < calibratedMin[i])
-            calibratedMin[i] = min_sensor_values[i];
-        if(max_sensor_values[i] > calibratedMax[i])
-            calibratedMax[i] = max_sensor_values[i];
-    }
-    pc.printf("calibration complete\r\n");  
-    flag =1;   
-}
-
-void tr_ready(void){
-    
-    while(1){
-        // read current IR 1 ~ IR 5
-        read_ir();
-        
-        // set range under 1000
-        set_ir_val();
-               
-        // get current position
-        get_pos();
-        
-//        pc.printf("pos = %d\r\n", pos);
-        // start tracing_pid_pid
-        //tracing_pid();
-        tracing();      
-//        pc.printf("pos = %d\r\n", pos);
-//        pc.printf("on_line = %d\r\n", on_line);
-                        
-//        wait(1);
-    }             
-}
-
-void set_ir_val(){
-    for(int i=0; i < NUM_SENSORS; i++)
-    {
-        unsigned int denominator;
-
-        denominator = calibratedMax[i] - calibratedMin[i];
-
-        signed int x = 0;
-        if(denominator != 0)
-            x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
-        if(x < 0)   x = 0;
-        else if(x > 1000)   x = 1000;
-        
-        sensor_values[i] = x;
-   }
-//   for(int i = 0; i <NUM_SENSORS; i++){
-//        pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
-//    }
-}
-
-void get_pos(){
-    on_line = 0;
-    avg = 0;
-    sum = 0;   
-
-    for(int i = 0; i < NUM_SENSORS; i++){
-        int val = sensor_values[i];
-        
-        // determine "on_line" or "out_line"        
-        if(val < 800){
-            on_line = 1;
-        }
-    
-        // under 
-        if(val > 5){
-            avg += (long)(val) * (i * 1000);
-            sum += val; 
-        }    
-    }
-    
-    // out_line
-    if(!on_line){
-        if(pos < (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
-            pos = 0;                             // last_vlaue = 0
-        }   
-        else{                                           // right -> out-line (over 2000)
-             pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
-        }
-    }
-    // on_line
-    else{
-        pos = avg / sum;        
-    }
-    pc.printf("position: %d\r\n", pos);
-    start = 1;
-}
-
-void tracing(){
-    // totally right
-    if(pos == 4000){
-        motor.speedup_r(3);
-    }
-    // detect IR1 on line
-    else if(pos >= 2300){
-        motor.speedup_r(3);    
-    }
-    // detect IR2 on line
-    else if(pos >= 2100){
-        motor.speedup_r(1);
-    }
-    // detect IR3 on line
-    else if(pos >= 1900){
-        motor.speed_r(0.09);
-        motor.speed_l(0.085);
-    }
-    // detect IR4 on line
-    else if(pos >= 1700){
-        motor.speedup_l(1);    
-    }
-    // detect IR5 on line
-    else if(pos >= 1200){
-        motor.speedup_l(3);    
-    }
-    // totally left
-    else if(pos == 0){
-        motor.speedup_l(3);            
-    }
-    else {
-        pc.printf("error pos: %d\r\n", pos);
-        pc.printf("error\r\n");
-    }
-    wait(0.001);
-
-}
-
-void tracing_pid(void){
-
-    int proportional = pos - 2000;
-    
-    int derivative = proportional - last_proportional;
-    integral += proportional;
-    
-    last_proportional  = proportional;
-    
-    int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
-//    int power_difference = proportional / ;
-    
-    const int max = 150;
-    
-    if(power_difference > max)
-        power_difference = max;
-        
-    if(power_difference < -max)
-        power_difference = -max;
-    
-    if(power_difference < 0){
-        motor.speed_l((max + power_difference)/255);
-        motor.speed_r(max/255);
-    }
-    else{
-        motor.speed_l(max/255);
-        motor.speed_r((max - power_difference)/255);
-    }
-    
-    pc.printf("proportional: %d\r\n", proportional);
-    pc.printf("power_difference: %d\r\n", power_difference);
-    pc.printf("derivative: %d\r\n", derivative);    
-    pc.printf("integral: %d\r\n",integral); 
-}
+#include "mbed.h"
+//#include "motor.h"
+#include "ReceiverIR.h"
+#include "Adafruit_SSD1306.h"   // OLED
+#define NUM_SENSORS 5
+ 
+//Motor motor(D6, D5, PA_0, PA_1, PB_0, PA_4, 0.3);
+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);
+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);
+ 
+Thread TR_thread;
+Thread remote_thread;
+Thread oled_thread;
+ 
+unsigned long avg = 0;
+unsigned int sum = 0;
+ 
+unsigned int sensor_values[NUM_SENSORS];
+unsigned int max_sensor_values[NUM_SENSORS];
+unsigned int min_sensor_values[NUM_SENSORS];
+ 
+unsigned int *calibratedMin;
+unsigned int *calibratedMax;
+ 
+int value;
+float bat;
+int on_line = 0;
+static int pos = 0;
+unsigned int last_proportional = 0;
+long integral = 0;
+ 
+volatile int flag = 0;
+volatile int start = 0;
+ 
+int ch;
+ 
+ 
+ 
+int readLine(unsigned int *sensor_values);
+void tr_ready(void);
+void calibration(void);
+void init(void);
+ 
+void tracing_pid(void);
+void set_ir_val(void);
+void get_pos(void);
+int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
+void remote_ctrl(void);
+void oled_disp(void);
+ 
+int main()
+{
+    pc.printf("start\r\n");
+    spi.format(16, 0);
+    spi.frequency(2000000);
+ 
+    init();
+ 
+    oled_thread.start(&oled_disp);
+    remote_thread.start(&remote_ctrl);
+    while(!motor.cal_start);
+ 
+    calibration();
+ 
+    while(!flag);
+ 
+    TR_thread.start(&tr_ready);
+ 
+ 
+    while(!start);
+ 
+    pc.printf("motor forward\r\n");
+    motor.forward();
+ 
+    while (1);
+}
+ 
+void remote_ctrl(void){
+ 
+    while(1){
+    uint8_t buf1[32];
+    uint8_t buf2[32];
+    int bitlength1;
+    RemoteIR::Format format;
+ 
+    memset(buf1, 0x00, sizeof(buf1));
+    memset(buf2, 0x00, sizeof(buf2));
+ 
+        bitlength1 = receive(&format, buf1, sizeof(buf1));
+        if (bitlength1 < 0) {
+            continue;
+        }
+ 
+    }
+}
+ 
+void init(void){
+    motor.cal_start = 0;
+    motor.cal_stop = 0; 
+    
+    motor.kp = 10;
+    motor.kd = 2;
+    motor.max = 100;
+ 
+    DataComm = 0;
+ 
+    calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
+    calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
+ 
+    for(int i=0; i < NUM_SENSORS; i++)
+    {
+      calibratedMin[i] = 1023;
+      calibratedMax[i] = 0;
+    }
+}
+ 
+void oled_disp(void){
+    myOled.begin();
+    while(1){
+        myOled.clearDisplay();
+        //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
+        myOled.printf("kp:%.2f, kd: %d\r", motor.kp, motor.kd);
+        myOled.display();
+    }
+}
+ 
+ 
+int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout) {
+    int cnt = 0;
+    while (motor.getState() != ReceiverIR::Received) {
+        cnt++;
+        if (timeout < cnt) {
+            return -1;
+        }
+    }
+    return motor.getData(format, buf, bufsiz * 8);
+}
+ 
+ 
+void read_ir(void){
+    ch = 0;
+ 
+    spi_cs = 0;
+    wait_us(2);
+    value = spi.write(ch << 12);
+    spi_cs = 1;
+ 
+    wait_us(21);
+ 
+    for(int i = 0; i < 5; i++){
+ 
+        ch += 1;
+ 
+        spi_cs = 0;
+        wait_us(2);
+        value = spi.write(ch << 12);
+        spi_cs = 1;
+ 
+        wait_us(21);
+ 
+        value = value >> 6;
+        value = value * 3300 / 0x3FF;
+ 
+        sensor_values[i] = value;
+    }
+}
+ 
+void calibration(void){
+    pc.printf("calibration start \r\n");
+    
+    int i = 0;
+    while(!motor.cal_stop){    
+        read_ir();
+        for(int j = 0; j < NUM_SENSORS; j++){
+            if(i == 0 || max_sensor_values[j] < sensor_values[j]){
+                max_sensor_values[j] = sensor_values[j];
+            }
+ 
+            if(i == 0 || min_sensor_values[j] > sensor_values[j]){
+                min_sensor_values[j] = sensor_values[j];
+            }
+        }
+        i = 1;
+    }
+ 
+    for(int i = 0; i < NUM_SENSORS; i++){
+        if(min_sensor_values[i] < calibratedMin[i])
+            calibratedMin[i] = min_sensor_values[i];
+        if(max_sensor_values[i] > calibratedMax[i])
+            calibratedMax[i] = max_sensor_values[i];
+    }
+    
+    pc.printf("calibration complete\r\n");
+    flag = 1;
+}
+ 
+void tr_ready(void){
+ 
+    while(1){
+        // read current IR 1 ~ IR 5
+        read_ir();
+ 
+        // set range under 1000
+        set_ir_val();
+ 
+        // get current position
+        get_pos();
+ 
+        tracing_pid();
+        
+        
+    }
+}
+ 
+void set_ir_val(){
+    for(int i = 0; i < NUM_SENSORS; i++)
+    {
+        unsigned int denominator;
+ 
+        denominator = calibratedMax[i] - calibratedMin[i];
+ 
+        signed int x = 0;
+        if(denominator != 0)
+            x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
+        if(x < 0)   x = 0;
+        else if(x > 1000)   x = 1000;
+ 
+        sensor_values[i] = x;
+        
+   }
+   
+   // finish line
+   if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
+        motor.stop();
+        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(){
+    on_line = 0;
+    avg = 0;
+    sum = 0;
+ 
+    for(int i = 0; i < NUM_SENSORS; i++){
+        int val = sensor_values[i];
+ 
+        // determine "on_line" or "out_line"
+        if(val < 500){
+            on_line = 1;
+        }
+ 
+        // under
+        if(val > 5){
+            avg += (long)(val) * (i * 1000);
+            sum += val;
+        }
+    }
+ 
+    // out_line
+    if(!on_line){
+        if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
+            pos = 0;                             // last_vlaue = 0
+        }
+        else{                                    // right -> out-line (over 2000)
+             pos = (NUM_SENSORS - 1) * 1000;     // pos = 4000
+        }
+    }
+    // on_line
+    else{
+        pos = avg / sum;
+    }
+ 
+    start = 1;
+}
+ 
+void tracing_pid(void){
+ 
+    
+    int proportional = pos - 2000;
+    
+    int derivative = proportional - last_proportional;
+    integral += proportional;
+ 
+    last_proportional  = proportional;
+ 
+    //int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
+    //int power_difference = proportional / motor.kp;
+    int power_difference = proportional/motor.kp + derivative * motor.kd; 
+    const float std = 600.0;
+ 
+    if(power_difference > motor.max)
+        power_difference = motor.max;
+ 
+    if(power_difference < -motor.max)
+        power_difference = -motor.max;
+    
+    // bot position: right
+    if(power_difference > 15){
+        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((motor.max + power_difference)/std);
+        motor.speed_l((motor.max - power_difference)/std);
+    }
+    else{ //online
+        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