embedded
190605
Dependencies: WS2812 RemoteIR PixelArray Adafruit_GFX
Diff: main.cpp
- Revision:
- 99:d8123dfcf757
- Parent:
- 97:bad45e2dc7e1
--- a/main.cpp Wed Jun 05 04:55:45 2019 +0000
+++ b/main.cpp Wed Jun 05 13:56:18 2019 +0000
@@ -9,7 +9,7 @@
SPI spi(D11, D12, D13);
DigitalOut spi_cs(D10, 1);
Serial pc(SERIAL_TX, SERIAL_RX, 115200);
-I2C i2c(D14, D15); // D14, D15
+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);
@@ -22,10 +22,10 @@
unsigned int sensor_values[NUM_SENSORS];
unsigned int max_sensor_values[NUM_SENSORS];
-unsigned int min_sensor_values[NUM_SENSORS];
+unsigned int min_sensor_values[NUM_SENSORS];
unsigned int *calibratedMin;
-unsigned int *calibratedMax;
+unsigned int *calibratedMax;
int value;
float bat;
@@ -39,12 +39,14 @@
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 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);
@@ -52,34 +54,34 @@
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);
-
+ 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");
+
+ pc.printf("motor forward\r\n");
motor.forward();
-
+
while (1);
}
void remote_ctrl(void){
-
+
while(1){
uint8_t buf1[32];
uint8_t buf2[32];
@@ -93,17 +95,19 @@
if (bitlength1 < 0) {
continue;
}
-
- //display_status("RECV", bitlength1);
- //display_data(buf1, bitlength1);
- //display_format(format);
- }
+ }
}
void init(void){
+ motor.cal_start = 0;
+ motor.cal_stop = 0;
+
+ motor.kp = 10;
+ motor.max = 130;
+
DataComm = 0;
-
+
calibratedMin = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
calibratedMax = (unsigned int*)malloc(sizeof(unsigned int) * NUM_SENSORS);
@@ -118,8 +122,9 @@
myOled.begin();
while(1){
myOled.clearDisplay();
- myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
- myOled.display();
+ //myOled.printf("%.3f, %.3f, %d\r", motor.Speed_L, motor.Speed_R, pos);
+ myOled.printf("kp:%.2f, max:%d\r", motor.kp, motor.max);
+ myOled.display();
}
}
@@ -137,28 +142,28 @@
void read_ir(void){
ch = 0;
-
+
spi_cs = 0;
wait_us(2);
value = spi.write(ch << 12);
spi_cs = 1;
-
- wait_us(21);
-
+
+ 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);
-
+
+ wait_us(21);
+
value = value >> 6;
value = value * 3300 / 0x3FF;
-
+
sensor_values[i] = value;
}
}
@@ -166,55 +171,52 @@
void calibration(void){
pc.printf("calibration start \r\n");
- for(int i = 0; i < 10; i++){
+ 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];
}
}
- wait(0.5);
+ 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;
+
+ 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();
-// 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++)
+ for(int i = 0; i < NUM_SENSORS; i++)
{
unsigned int denominator;
@@ -225,120 +227,100 @@
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;
- sensor_values[i] = x;
}
-// for(int i = 0; i <NUM_SENSORS; i++){
-// pc.printf("sensor_Values[%d] : %d\r\n", i, sensor_values[i]);
-// }
+
+ // 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;
+ sum = 0;
for(int i = 0; i < NUM_SENSORS; i++){
int val = sensor_values[i];
-
- // determine "on_line" or "out_line"
- if(val < 800){
+
+ // determine "on_line" or "out_line"
+ if(val < 500){
on_line = 1;
}
-
- // under
+
+ // under
if(val > 5){
avg += (long)(val) * (i * 1000);
- sum += val;
- }
+ sum += val;
+ }
}
-
+
// out_line
if(!on_line){
- if(pos < (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
+ if(pos <= (NUM_SENSORS - 1) * 1000 / 2) { // left -> out-line (under 2000)
pos = 0; // last_vlaue = 0
- }
- else{ // right -> out-line (over 2000)
+ }
+ 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);
+ pos = avg / sum;
}
- // 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);
+ 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 / ;
-
- const int max = 150;
-
- if(power_difference > max)
- power_difference = max;
-
- if(power_difference < -max)
- power_difference = -max;
+
+ //int power_difference = proportional / 20 + integral / 10000 + derivative * 10;
+ int power_difference = proportional / motor.kp;
+
+ const float std = 1000;
+
+ if(power_difference > motor.max)
+ power_difference = motor.max;
+
+ if(power_difference < -motor.max)
+ power_difference = -motor.max;
- if(power_difference < 0){
- motor.speed_l((max + power_difference)/255);
- motor.speed_r(max/255);
+ // bot position: right
+ if(power_difference > 15){
+ motor.speed_r((motor.max + power_difference)/std);
+ motor.speed_l(motor.max/33);
+ }
+ // bot position: left
+ else if(power_difference < -15){
+ motor.speed_r(motor.max/std);
+ motor.speed_l((motor.max - power_difference)/std);
}
- else{
- motor.speed_l(max/255);
- motor.speed_r((max - power_difference)/255);
+ 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("power_difference: %d\r\n", power_difference);
- pc.printf("derivative: %d\r\n", derivative);
- pc.printf("integral: %d\r\n",integral);
-}
+
+ // 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