embedded
190605
Dependencies: WS2812 RemoteIR PixelArray Adafruit_GFX
Diff: main.cpp
- Revision:
- 107:94134c6f90e8
- Parent:
- 106:d14f340f1fe0
- Child:
- 108:64e7d7025e2f
--- a/main.cpp Mon Jun 10 11:10:06 2019 +0000
+++ b/main.cpp Wed Jun 12 07:47:47 2019 +0000
@@ -13,7 +13,6 @@
#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);
@@ -124,6 +123,11 @@
msec_t %= 1000;
sec_t = timer_s.read();
+ if(sec_t > 60){
+ min_t = 1;
+ sec_t %= 60;
+ }
+
while (1);
}
@@ -169,11 +173,10 @@
motor.cal_start = 0;
motor.cal_stop = 0;
- motor.kp = 0.1;
- motor.kd = 0.0;
- motor.ki = 5000;
- motor.max = 200;
- motor.standard = 500.0;
+ motor.kp = 10.0;
+ motor.kd = 1.0;
+// motor.ki = 5000;
+ motor.max = 100;
DataComm = 0;
@@ -204,15 +207,10 @@
void oled_disp(void){
myOled.begin();
while(1){
- myOled.printf("%.2f %.2f %.f %.f\r", motor.kp, motor.kd, motor.max, motor.standard);
+ myOled.printf("timer: %d:%d:%d sec\r",min_t, sec_t, msec_t);
// 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();
+
}
}
@@ -255,11 +253,7 @@
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");
+
}
@@ -303,11 +297,11 @@
// get current position
get_pos();
-// pc.printf("pos: %d\r\n", pos);
+ pc.printf("pos: %d\r\n", pos);
+ pc.printf("on_line: %d\r\n\n", on_line);
+
tracing_pid();
-
- //control
-// control();
+
}
}
@@ -323,35 +317,25 @@
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;
+ }
}
@@ -364,7 +348,7 @@
int val = sensor_values[i];
// determine "on_line" or "out_line"
- if(val < 500){
+ if(val < 450){
on_line = 1;
}
@@ -377,13 +361,11 @@
// out_line
if(!on_line){
- //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
+ 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
- pos = 3800;
+ pos = (NUM_SENSORS - 1) * 1000; // pos = 4000
}
}
// on_line
@@ -396,66 +378,37 @@
void tracing_pid(void){
- int proportional = pos - 2050;
+ int proportional = pos - 2000;
- int derivative = proportional - last_proportional;
-
+ int derivative = proportional - last_proportional;
integral += proportional;
-
+
last_proportional = proportional;
- int power_difference = proportional*motor.kp + derivative * motor.kd;
-
-
+ int power_difference = proportional / motor.kp+ derivative * motor.kd;
+
+ const float std = 990.0;
+
if(power_difference > motor.max)
power_difference = motor.max;
if(power_difference < -motor.max)
power_difference = -motor.max;
- //완전 online일 때 init_spl, init_spr
- motor.speed_r((motor.max + power_difference)/motor.standard);
- motor.speed_l((motor.max - power_difference)/motor.standard);
+ // 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);
+ }
+ // 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);
+ }
}
-//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