Yi Lin Chen
shared
Diff: main.cpp
- Revision:
- 7:fe8665daf3e7
- Parent:
- 6:132facf9b42d
- Child:
- 8:089b778962c4
--- a/main.cpp Tue Jul 01 16:33:34 2014 +0000
+++ b/main.cpp Mon Jul 07 06:52:03 2014 +0000
@@ -1,16 +1,15 @@
#include "mbed.h"
#include "servo_api.h"
#include "camera_api.h"
-
+#include "motor_api.h"
#define Debug_cam_uart
-
-
+#define buffer_size 3
Serial pc(USBTX, USBRX);
BX_servo servo;
-
- BX_camera cam;
-
+BX_camera cam;
+BX_motor motorA('A');
+BX_motor motorB('B');
int main() {
@@ -19,74 +18,303 @@
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){
- cam.read();
-
- for(int i=0;i<128;i++){
- if(i==64)
- pc.printf("X");
- else
- pc.printf("%c", cam.sign_line_imageL[i]);
- }
- pc.printf(" || ");
-
- for(int i=0;i<128;i++){
- if(i==64)
- pc.printf("X");
- else
- pc.printf("%c", cam.sign_line_imageR[i]);
- }
- pc.printf("\r\n");
-
-
-
- pc.printf("R center : %d \r\n",cam.black_centerR());
-
-
+ 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
+ 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;
+ }
+ }
+ }
+
+ //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];
+
+ }
+ 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;
-
-
-
-
- // pc.printf("ang :%d\r\n ",( (64.0-center) /64.0 )*90);
- //--------------------------------------------
-
-
- // servo.set_angle(( (64.0-center) /64.0 )*90 );
-
-
-
-
-
-
- #endif
-
-
-
-
-
-
-
-
-
-
-
-
+ //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;
+ }
+ //====== 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_average_point = 0;
+ int right_average_length = 0;
+ //check line
+ bool isRightAllWhite = true;
+ int right_black_count = 0;
+ for(int i=10;i<=117;i++){
+ if(cam.sign_line_imageR[i] == ' ')
+ {
+ right_black_count++;
+ if( right_black_count >= 3 )
+ {
+ isRightAllWhite = false;
+ }
+ }
+ }
+
+ 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];
+ }
+ 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_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];
+ }
+ 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");
+ }
+
+ //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");
+ }
- return 0;
+ pc.printf("\n L_center : %d , R_center : %d \r\n", left_average_point,right_average_point);
+ pc.printf("\r\n");
+ //PID -- P
+ int error = 0;
+ int turn = 0;
+ int center = 55;
+ float Kp = 0;
+ float turn_angle = 0;
+ int reference_point= 0;
+
+ if(isLeftAllWhite && !isRightAllWhite){
+ error = 40;
+ turn = 90;
+ reference_point = right_average_point;
+ }
+ else if(!isLeftAllWhite && isRightAllWhite){
+ error = 40;
+ turn = 90;
+ 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");
+
+ }
+ else if(!isLeftAllWhite && !isRightAllWhite){
+ error = 50;
+ turn = 90;
+ center = 108;
+ reference_point = (right_average_point + 108 + left_average_point)/2;
+ }
-}
+ Kp = turn/error;
+
+ //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);*/
+
+ turn_angle = (int) (Kp*(reference_point - center));
+ if(turn_angle >= 50)
+ turn_angle = 50;
+ if(turn_angle <= -50)
+ turn_angle = -50;
+
+ servo.set_angle(turn_angle);
+ }
+
+
+
+ #endif
+
+ return 0;
+
+}
\ No newline at end of file