embedded
190605
Dependencies: WS2812 RemoteIR PixelArray Adafruit_GFX
Diff: main.cpp
- Revision:
- 106:d14f340f1fe0
- Parent:
- 104:94ed54c74466
- Child:
- 107:94134c6f90e8
--- a/main.cpp Sat Jun 08 17:25:05 2019 +0000
+++ b/main.cpp Mon Jun 10 11:10:06 2019 +0000
@@ -12,12 +12,12 @@
#define WS2812_BUF 10
#define NUM_COLORS 6
#define NUM_LEDS_PER_COLOR 4
-
+
//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);
+RawSerial pc(SERIAL_TX, SERIAL_RX, 115200);
I2C i2c(I2C_SDA, I2C_SCL); // D14, D15
Adafruit_SSD1306_I2c myOled(i2c, D9, 0x78, 64, 128); // D9, (D8: D/C - data/command change)
DigitalOut DataComm(D8);
@@ -28,7 +28,6 @@
Thread TR_thread;
Thread remote_thread;
Thread oled_thread;
-Thread photo_obstacle_thread;
// Timer
@@ -47,7 +46,6 @@
unsigned int *calibratedMax;
int value;
-float bat;
int on_line = 0;
static int pos = 0;
unsigned int last_proportional = 0;
@@ -56,7 +54,6 @@
volatile int flag = 0;
volatile int start = 0;
volatile int timer_flag = 0;
-volatile int detect = 0;
int ch;
@@ -73,6 +70,8 @@
int status;
int count_stop;
+int err;
+int pid;
int readLine(unsigned int *sensor_values);
void tr_ready(void);
@@ -86,9 +85,9 @@
int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100);
void remote_ctrl(void);
void oled_disp(void);
-void photo_detect(void);
void bottom_led(void);
+void control(void);
int main()
{
@@ -102,7 +101,6 @@
oled_thread.start(&oled_disp);
remote_thread.start(&remote_ctrl);
- photo_obstacle_thread.start(&photo_detect);
while(!motor.cal_start);
@@ -171,11 +169,11 @@
motor.cal_start = 0;
motor.cal_stop = 0;
- motor.kp = 10.0;
- motor.kd = 1.0;
+ motor.kp = 0.1;
+ motor.kd = 0.0;
motor.ki = 5000;
- motor.max = 250;
-
+ motor.max = 200;
+ motor.standard = 500.0;
DataComm = 0;
@@ -202,57 +200,19 @@
}
}
-
-void photo_detect(void){
-
- while(1){
- pc.printf("detect: %d\r\n", detect);
-
- i2c.read(((PCF8574_ADDR << 1) | 0x01), data_read, 1, 0);
-
- int dsl = (int)data_read[0] & 0x80; //1000_0000
- int dsr = (int)data_read[0] & 0x40; //0100_0000
-
- if(dsl == 0){
- detect = 1;
- motor.stop();
- wait_ms(30);
- motor.speed_l(0.6);
- motor.speed_r(0);
- wait_ms(300);
- motor.speed_l(0.2);
- motor.speed_r(0.2);
- wait_ms(50);
- motor.speed_l(0);
- motor.speed_r(0.7);
- wait_ms(200);
- detect = 0;
- }
- else if(dsr == 0){
- detect = 1;
- motor.speed_l(0);
- motor.speed_r(0.4);
- wait_ms(300);
- motor.speed_l(0.2);
- motor.speed_r(0.2);
- wait_ms(50);
- motor.speed_l(0.6);
- motor.speed_r(0);
- wait_ms(200);
- detect = 0;
- }
-
- }
-}
void oled_disp(void){
myOled.begin();
while(1){
- myOled.clearDisplay();
- //myOled.printf("%.2f\r", motor.kd);
- myOled.printf("time: %u:%u.%u\r", min_t, sec_t, msec_t);
-// myOled.printf("%.2f %.2f %d %d\r", motor.kp, motor.kd, motor.ki, motor.max);
+ myOled.printf("%.2f %.2f %.f %.f\r", motor.kp, motor.kd, motor.max, motor.standard);
+// myOled.printf("%spl: %.3f spr:%.3f\r", motor.base_spl, motor.base_spr);
myOled.display();
+ // myOled.clearDisplay();
+// wait(0.3);
+// myOled.begin();
+// myOled.printf("%d %.0f\r", motor.max, motor.standard);
+// myOled.display();
+// myOled.clearDisplay();
}
}
@@ -295,6 +255,12 @@
sensor_values[i] = value;
}
+
+// for(int i = 0; i < 5; i++){
+// pc.printf("raw sensor values[%d]: %d\r\n",i, sensor_values[i]);
+// }
+// pc.printf("\r\n");
+
}
void calibration(void){
@@ -331,15 +297,17 @@
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);
+ tracing_pid();
- while(detect);
- tracing_pid();
+ //control
+// control();
}
}
@@ -355,24 +323,35 @@
x = (((signed long)sensor_values[i]) - calibratedMin[i]) * 1000 / denominator;
if(x < 0) x = 0;
else if(x > 1000) x = 1000;
-
+//method2
+// if(x < 250) x = 1; //black
+// else x = 0;
+
sensor_values[i] = x;
-
+
}
+// for(int i = 0; i < NUM_SENSORS; i++){
+// pc.printf("(after)sensor_values[%d] = %d\r\n", i, sensor_values[i]);
+// }
+//
+// pc.printf("\r\n");
+// wait(0.5);
+
+
// finish line
- if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
- count_stop ++;
- if(count_stop > 7){
- timer_flag = 1;
- motor.stop();
-
- bottom_led();
- }
- }
- else{
- count_stop = 0;
- }
+ //if(sensor_values[0] < 500 && sensor_values[1] < 500 && sensor_values[2] < 500 && sensor_values[3] < 500 && sensor_values[4] < 500){
+// count_stop ++;
+// if(count_stop > 7){
+// timer_flag = 1;
+// motor.stop();
+//
+// bottom_led();
+// }
+// }
+// else{
+// count_stop = 0;
+// }
}
@@ -398,11 +377,13 @@
// out_line
if(!on_line){
- if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
- pos = 0; // last_vlaue = 0
+ //if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
+ if(pos <= 1800){ // left -> out-line (under 2000)
+ pos = 300; // last_vlaue = 0
}
else{ // right -> out-line (over 2000)
- pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
+// pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
+ pos = 3800;
}
}
// on_line
@@ -415,18 +396,16 @@
void tracing_pid(void){
- int proportional = pos - 2000;
+ int proportional = pos - 2050;
- int derivative = proportional - last_proportional;
+ int derivative = proportional - last_proportional;
integral += proportional;
last_proportional = proportional;
- int power_difference = proportional / motor.kp + derivative * motor.kd;
-
- const float std = 1000.0;
-
+ int power_difference = proportional*motor.kp + derivative * motor.kd;
+
if(power_difference > motor.max)
power_difference = motor.max;
@@ -434,8 +413,49 @@
if(power_difference < -motor.max)
power_difference = -motor.max;
-
- motor.speed_r((motor.max + power_difference)/std);
- motor.speed_l((motor.max - power_difference)/std);
+ //완전 online일 때 init_spl, init_spr
+ motor.speed_r((motor.max + power_difference)/motor.standard);
+ motor.speed_l((motor.max - power_difference)/motor.standard);
+}
+//method2
+void control(void){
+ if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 1)){
+ err = 4;
+}
+ else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 1) && (sensor_values[4] == 1)){
+ err = 3;
+ }
+ else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 1) && (sensor_values[4] == 0)){
+ err = 2;
+ }
+ else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 1) && (sensor_values[3] == 1) && (sensor_values[4] == 0)){
+ err = 1;
+ }
+ else if((sensor_values[0] == 0) && (sensor_values[1] == 0) && (sensor_values[2] == 1) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+ err = 0;
+ }
+ else if((sensor_values[0] == 0) && (sensor_values[1] == 1) && (sensor_values[2] == 1) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+ err = -1;
+ }
+ else if((sensor_values[0] == 0) && (sensor_values[1] == 1) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+ err = -2;
+ }
+ else if((sensor_values[0] == 1) && (sensor_values[1] == 1) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+ err = -3;
+ }
+ else if((sensor_values[0] == 1) && (sensor_values[1] == 0) && (sensor_values[2] == 0) && (sensor_values[3] == 0) && (sensor_values[4] == 0)){
+ err = -4;
+ }
+ pid = err*motor.kp;
+
+ if(motor.base_spr < pid){
+ pid = motor.base_spr;
+ }
+ if( pid < - motor.base_spl){
+ pid = -motor.base_spl;
+ }
+
+ motor.speed_r(motor.base_spr - pid);
+ motor.speed_l(motor.base_spl + pid);
}
\ No newline at end of file