Yi Lin Chen
shared
Diff: main.cpp
- Revision:
- 8:089b778962c4
- Parent:
- 7:fe8665daf3e7
--- a/main.cpp Mon Jul 07 06:52:03 2014 +0000
+++ b/main.cpp Fri Oct 31 10:54:51 2014 +0000
@@ -1,317 +1,275 @@
#include "mbed.h"
+#include "rtos.h"
#include "servo_api.h"
#include "camera_api.h"
#include "motor_api.h"
-
+#include "TSISensor.h"
+#include "pot.h"
#define Debug_cam_uart
-#define buffer_size 3
+#define buffer_size 6
+#define e_buffer_size 50
+//os
+Mutex stdio_mutex;
+BX_pot pot1('1');
+TSISensor tsiSensor;
+
Serial pc(USBTX, USBRX);
BX_servo servo;
BX_camera cam;
BX_motor motorA('A');
BX_motor motorB('B');
+int left_point_buffer[buffer_size] = {0,0,0,0,0,0};
+int right_point_buffer[buffer_size] = {0,0,0,0,0,0};
+int error_buffer[e_buffer_size] ={0};
+float angle_buffer[50]={0};
+int state = 0; //0: no line, 1:right line, 2:left line
+int last_state = 0;
+float turnAngle = 0.073;
+float Kp = 0.0;
-int main() {
-
- /*
- int black_va;
- int white_va;
- */
- int left_point_buffer[buffer_size] = {0,0,0};
- int right_point_buffer[buffer_size] = {0,0,0};
- int left_length_buffer[buffer_size] = {0,0,0};
- int right_length_buffer[buffer_size] = {0,0,0};
- int error_buffer[buffer_size]= {0,0,0};
- bool isLeftOut = false;
- bool isRightOut = false;
- servo.set_angle(0);
-#ifdef Debug_cam_uart
- pc.baud(115200);
-
- motorA.rotate(0.5);
- motorB.rotate(0.5);
-
- while(1){
-
+void servo_thread(void const *args){
+ while(1){
+ for(int i=0;i<49;i++){
+ angle_buffer[i] = angle_buffer[i+1];
+ }
+ angle_buffer[49] = turnAngle;
+
+ float average_angle=0;
+ for(int i=25;i<50;i++){
+ average_angle=average_angle+angle_buffer[i];
+ }
+ average_angle = average_angle/25;
+ float speed_error = average_angle - 0.073;
+ int Km_speed = 0;;
+ if(speed_error<0)
+ speed_error = speed_error*(-1);
+
+ if(speed_error <= 0.004 || speed_error >= 0.01){
+ Km_speed = -13;
+ }
+ else{
+ Km_speed = -7;
+ }
+
+ float speed = Km_speed*speed_error;
+ int int_max = pot1.read()*10;
+ float max = int_max/(10.0);
+ if(max+speed<0)
+ speed = 0.2-max;
+ motorA.rotate(max+speed);
+ motorB.rotate(max+speed);
+
+ //pc.printf("\r\n Turn Angle = %f \r\n",turnAngle );
+ servo.set_angle(turnAngle);
+ Thread :: wait(30);
+ }
+}
+
+float find_error(){
cam.read();
-
- //====== find left line's right edging ======
- bool left_isFistPoint = true;
- int left_temp_length = 0;
- int left_max_length = 0;
- int left_temp_point = 0;
- int left_target_point = 0;
- int left_white_count = 0;
- int left_white_threshold = 2;
- // scan camera data from right to left
+ int left_target_point = 0;
+ int left_average_point = 0;
+ int left_black_count = 0;
+ bool isLeftAllWhite = true;
for(int i=117;i>=10;i--){
- // when scan result is black and it is a start point
- if(cam.sign_line_imageL[i] == ' ' && left_isFistPoint){
- left_temp_point = i;
- left_isFistPoint = false;
- if(i == 10 && (left_max_length < left_temp_length)){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- }
- // when scan result is black and it is not a start point
- else if(cam.sign_line_imageL[i] == ' ' && !left_isFistPoint){
- left_temp_length++;
- if(i == 10 && (left_max_length < left_temp_length)){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- }
- // when scan result is white and there is no black behind it
- else if(cam.sign_line_imageL[i] != ' ' && left_isFistPoint){
- //do nothng
- }
- //when scan result is white and there are some black behind it
- else if(cam.sign_line_imageL[i] != ' ' && !left_isFistPoint){
- left_white_count++;
- //this white is still in the threshold
- if(left_white_count <= left_white_threshold){
- left_temp_length++;
- if(i == 10 && (left_max_length < left_temp_length)){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- }
- //this white is out of threshold
- else{
- left_isFistPoint = true;
- if(left_max_length < left_temp_length){
- left_max_length = left_temp_length;
- left_target_point = left_temp_point;
- }
- left_temp_length = 0;
- left_white_count = 0;
- }
- }
+ if(cam.sign_line_imageL[i] == ' '){
+ left_black_count++;
+ if(left_black_count == 10){
+ left_target_point = i;
+ isLeftAllWhite = false;
+ break;
+ }
+ }
+ else{
+ left_black_count = 0;
+ }
}
- //average data
- int left_average_point = 0;
- int left_average_length = 0;
-
- //check line
- bool isLeftAllWhite = true;
- int left_black_count = 0;
- for(int i=10;i<=117;i++){
- if(cam.sign_line_imageL[i] == ' ')
- {
- left_black_count++;
- if( left_black_count >= 3 )
- {
- isLeftAllWhite = false;
- }
- }
- }
if(isLeftAllWhite){
//do not update the data
- //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2])/buffer_size;
- //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2])/buffer_size;
- left_average_point = left_point_buffer[buffer_size-1];
- left_average_length = left_length_buffer[buffer_size-1];
-
+ //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5])/buffer_size;
+ left_average_point = left_point_buffer[5];
}
else{
- //average the history data
- //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_target_point)/(buffer_size+1);
- //left_average_length = (left_length_buffer[0] + left_length_buffer[1] + left_length_buffer[2] + left_max_length)/(buffer_size+1);
-
- left_average_point = left_target_point;
- left_average_length = left_max_length;
-
- //update the buffer
- for(int i=0;i<buffer_size - 1;i++){
- left_point_buffer[i] = left_point_buffer[i+1];
- left_length_buffer[i] = left_length_buffer[i+1];
- }
- left_point_buffer[buffer_size-1] = left_average_point;
- left_length_buffer[buffer_size-1] = left_average_length;
+ //average the history data
+ //left_average_point = (left_point_buffer[0] + left_point_buffer[1] + left_point_buffer[2] + left_point_buffer[3] +left_point_buffer[4]+left_point_buffer[5]+ left_target_point)/(buffer_size+1);
+ left_average_point = left_target_point;
}
//====== find right line's left edging ======
- bool right_isFistPoint = true;
- int right_temp_length = 0;
- int right_max_length = 0;
- int right_temp_point = 0;
- int right_target_point = 0;
- int right_white_count = 0;
- int right_white_threshold = 2;
- //scan camera data from right to left
- for(int i=10;i<=117;i++){
- //when scan result is black and it is a start point
- if(cam.sign_line_imageR[i] == ' ' && right_isFistPoint){
- right_temp_point = i;
- right_isFistPoint = false;
- if( i == 117 && (right_max_length < right_temp_length)){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- }
- //when scan result is black and it is not a start point
- else if(cam.sign_line_imageR[i] == ' ' && !right_isFistPoint){
- right_temp_length++;
- if( i == 117 && (right_max_length < right_temp_length)){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- }
- //when scan result is white and there is no black behind it
- else if(cam.sign_line_imageR[i] != ' ' && right_isFistPoint){
- //do nothng
- }
- //when scan result is white and there are some black behind it
- else if(cam.sign_line_imageR[i] != ' ' && !right_isFistPoint){
- right_white_count++;
- //this white is still in the threshold
- if(right_white_count <= right_white_threshold){
- right_temp_length++;
- if( i == 117 && (right_max_length < right_temp_length)){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- }
- //this white is out of threshold
- else{
- right_isFistPoint = true;
- if(right_max_length < right_temp_length){
- right_max_length = right_temp_length;
- right_target_point = right_temp_point;
- }
- right_temp_length = 0;
- right_white_count = 0;
- }
- }
- }
-
- //average data
+ int right_target_point = 0;
int right_average_point = 0;
- int right_average_length = 0;
- //check line
+ int right_black_count = 0;
bool isRightAllWhite = true;
- int right_black_count = 0;
- for(int i=10;i<=117;i++){
- if(cam.sign_line_imageR[i] == ' ')
- {
+ for(int i=10;i<118;i++){
+ if(cam.sign_line_imageR[i] == ' '){
right_black_count++;
- if( right_black_count >= 3 )
- {
+ if(right_black_count == 10){
+ right_target_point = i;
isRightAllWhite = false;
- }
+ break;
+ }
+ }
+ else{
+ right_black_count = 0;
}
}
+
+ /*for(int i=118;i>=10;i--){
+ if(i<left_average_point){
+ pc.printf(" ");
+ }
+ else
+ pc.printf("O");
+ }*/
+
+
+
if(isRightAllWhite){
//do not update the buffer
- //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2])/buffer_size;
- //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2])/buffer_size;
- right_average_point = right_point_buffer[buffer_size-1];
- right_average_length = right_length_buffer[buffer_size-1];
+ //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5])/buffer_size;
+ right_average_point = right_point_buffer[5];
}
else{
//average the history data
- //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_target_point)/(buffer_size+1);
- //right_average_length = (right_length_buffer[0] + right_length_buffer[1] + right_length_buffer[2] + right_max_length)/(buffer_size+1);
+ //right_average_point = (right_point_buffer[0] + right_point_buffer[1] + right_point_buffer[2] + right_point_buffer[3] + right_point_buffer[4] + right_point_buffer[5] + right_target_point)/(buffer_size+1);
right_average_point = right_target_point;
- right_average_length = right_max_length;
-
- //update the buffer
- for(int i=0;i<buffer_size-1;i++){
- right_point_buffer[i] = right_point_buffer[i+1];
- right_length_buffer[i] = right_length_buffer[i+1];
+ }
+ /*
+ for(int i=10;i<118;i++){
+ if(i>=right_average_point){
+ pc.printf(" ");
}
- right_point_buffer[buffer_size-1] = right_average_point;
- right_length_buffer[buffer_size-1] = right_average_length;
- }
-
- //print left camera result
-
- for(int i=0;i<=127;i++){
- if( i >= left_average_point-left_average_length && i<=left_average_point)
- pc.printf(" ");
else
- pc.printf("x");
+ pc.printf("O");
+ }
+ pc.printf("\r\n");*/
+ //update buffer
+ for(int i=0;i<buffer_size-1;i++){
+ right_point_buffer[i] = right_point_buffer[i+1];
+ left_point_buffer[i] = left_point_buffer[i+1];
}
- //print right camera result
-
- for(int i=0;i<=127;i++){
- if( i <= right_average_point+right_average_length && i>=right_average_point)
- pc.printf(" ");
- else
- pc.printf("x");
- }
-
-
- pc.printf("\n L_center : %d , R_center : %d \r\n", left_average_point,right_average_point);
- pc.printf("\r\n");
+ left_point_buffer[buffer_size-1] = left_average_point;
+ right_point_buffer[buffer_size-1] = right_average_point;
- //PID -- P
- int error = 0;
- int turn = 0;
- int center = 55;
- float Kp = 0;
- float turn_angle = 0;
- int reference_point= 0;
-
+ int center = 0;
+ int reference_point = 0;
if(isLeftAllWhite && !isRightAllWhite){
- error = 40;
- turn = 90;
+ state = 1;
+ Kp = 0.0004;
+ center = 102;
reference_point = right_average_point;
+
}
else if(!isLeftAllWhite && isRightAllWhite){
- error = 40;
- turn = 90;
+ state = 2;
+ Kp = 0.0004;
+ center = 20;
reference_point = left_average_point;
}
else if(isLeftAllWhite && isRightAllWhite){
- error = 30;
- turn = 90;
- if(left_average_point < 40 && right_average_point < 40)
- reference_point = 0;
- else if(left_average_point > 70 && right_average_point > 70)
- reference_point = 90;
-
- //pc.printf("\n allllllllllllllllllllll whiteeeeeeeeeeeeeee \n");
+ state = 0;
+ Kp = 0.0004;
+ center = 64;
+ //straight line
+ float average_angle;
+ for(int i=0;i<50;i++){
+ average_angle = average_angle + angle_buffer[i];
+ }
+ average_angle = average_angle/50;
+ if(average_angle > 0.064 && average_angle < 0.08){
+ Kp = 0;
+ }
+ else{
+ if(left_average_point <= 40 && right_average_point <= 40)
+ reference_point = 15;
+ else if(left_average_point >= 70 && right_average_point >= 70)
+ reference_point = 95;
+ }
}
else if(!isLeftAllWhite && !isRightAllWhite){
- error = 50;
- turn = 90;
- center = 108;
- reference_point = (right_average_point + 108 + left_average_point)/2;
+ Kp = 0.0002;
+
+ if(128 - left_average_point >= right_average_point)
+ {
+ state = 1;
+ center = 95;
+ reference_point = right_average_point;
+ }
+ else{
+ state = 2;
+ center = 15;
+ reference_point = left_average_point;
+ }
+ }
+
+
+ return reference_point - center;
+}
+
+
+int main() {
+
+ while (1)
+ {
+ if (tsiSensor.readPercentage() > 0.00011)
+ {
+ Thread :: wait(2000);
+ break;
+ }
}
-
- Kp = turn/error;
+
+ servo.set_angle(0.073);
+
+#ifdef Debug_cam_uart
+ pc.baud(115200);
+
- //PID -- I
- /*for(int i=0;i<4;i++){
- error_buffer[i] = error_buffer[i+1];
- }
- error_buffer[4] = reference_point - center;
- float integral = 0;
- float Ki = 1.1f;
- for(int i=0;i<5;i++){
- integral = (2/3)*integral + error_buffer[i];
- }
- turn_angle = (int) (Kp*(reference_point - center) + Ki*integral);*/
+
+
+ motorA.rotate(0.33);
+ motorB.rotate(0.33);
+ Thread th_s(servo_thread);
+ while(1){
+ if(last_state != state){
+ for(int i=0;i<e_buffer_size - 1;i++){
+ error_buffer[i] = 0;
+ }
+ }
+ else{
+ for(int i=0;i<e_buffer_size - 1;i++){
+ error_buffer[i] = error_buffer[i+1];
+ }
+ }
+ error_buffer[e_buffer_size-1] = find_error();
- turn_angle = (int) (Kp*(reference_point - center));
- if(turn_angle >= 50)
- turn_angle = 50;
- if(turn_angle <= -50)
- turn_angle = -50;
+ float past_error = 0;
+ for(int i= 0;i<e_buffer_size;i++){
+ past_error = (past_error*2)/3 + error_buffer[i];
+ }
+ past_error = past_error/e_buffer_size;
+ float future_error = (error_buffer[e_buffer_size-1] - error_buffer[0])/e_buffer_size + error_buffer[e_buffer_size-1];
- servo.set_angle(turn_angle);
- }
-
-
+ stdio_mutex.lock();
+ turnAngle = 0.073 + Kp*(error_buffer[e_buffer_size-1]
+ + (1*past_error)/9
+ + (4*future_error)/9);
+ stdio_mutex.unlock();
+ last_state = state;
+
+
+ // pc.printf("%f \r\n",pot1.read());
+
+
+ Thread :: wait(5);
+ }
+
#endif