Clark Lin
/
BX-car_s
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 23:d6d4e8adf7fe, committed 2014-07-02
- Comitter:
- physicsgood
- Date:
- Wed Jul 02 13:33:49 2014 +0000
- Parent:
- 22:1464a3f0a290
- Commit message:
- QQQQ
Changed in this revision
diff -r 1464a3f0a290 -r d6d4e8adf7fe camera_api.cpp
--- a/camera_api.cpp Tue Jul 01 13:09:06 2014 +0000
+++ b/camera_api.cpp Wed Jul 02 13:33:49 2014 +0000
@@ -35,7 +35,7 @@
int BX_camera::black_centerR(void)//64~0 right eye
{
int black_R_left = 0;
- for(int i = 64; i >=6; i--){
+ for(int i = 120; i >=8; i--){
if(sign_line_imageR[i] == 'O' && sign_line_imageR[i-1] == ' ' && sign_line_imageR[i-2] == ' '){
return i;
}
@@ -45,8 +45,8 @@
int BX_camera::black_centerL(void)
{
int black_L_right = 128;
- for(int i = 64; i <123 ; i++){
- if(sign_line_imageR[i] == 'O' && sign_line_imageR[i+1] == ' ' && sign_line_imageR[i+2] == ' '){
+ for(int i = 8; i <121 ; i++){
+ if(sign_line_imageL[i] == 'O' && sign_line_imageL[i+1] == ' ' && sign_line_imageL[i+2] == ' '){
return i;
}
}
diff -r 1464a3f0a290 -r d6d4e8adf7fe controller.cpp
--- a/controller.cpp Tue Jul 01 13:09:06 2014 +0000
+++ b/controller.cpp Wed Jul 02 13:33:49 2014 +0000
@@ -7,7 +7,7 @@
PID::PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval) {
//BX tune
- Kp = 0.0004;
+ Kp = 0.0005;
Ki = 0.0;
Kd = 0.0;
setInputLimits(in_min,in_max);
@@ -120,14 +120,117 @@
-float PID::compute(float center , float sp) {
+float PID::compute(int centerL, int centerR , int sp) {
//turn right 122~64 122
//turn left 64~6 8
- float C = center;
- float goal = sp; // center of black
- float error = goal - C;//
- return 0.073+Kp*error;
-
+ int errorR = sp - centerR;
+ int errorL = sp - centerL;
+ int error;
+ if(centerL ==128 && centerR == 0){
+ error = 0;
+ }
+ if (centerL == 128 && centerR != 0){
+ error = -30;
+ }
+ else if (centerL != 128 && centerR == 0){
+ error = 30;
+ }
+ else{
+ if(errorR >= 0 && errorL >=0){
+ if(errorR > errorL){
+ if(errorR > -5 && errorR < 5)
+ error = 0;
+ else
+ error = errorR;
+ }
+ else{
+ if(errorL > -5 && errorL < 5)
+ error = 0;
+ else
+ error = errorL;
+ }
+
+ }
+ else if(errorR < 0 && errorL >=0){
+ if(-errorR > errorL){
+ if(errorR > -5 && errorR < 5)
+ error = 0;
+ else
+ error = errorR;
+ }
+ else{
+ if(errorL > -5 && errorL < 5)
+ error = 0;
+ else
+ error = errorL;
+ }
+ }
+ else if(errorR >= 0 && errorL <0){
+ if(errorR > -errorL){
+ if(errorR > -5 && errorR < 5)
+ error = 0;
+ else
+ error = errorR;
+ }
+ else{
+ if(errorL > -5 && errorL < 5)
+ error = 0;
+ else
+ error = errorL;
+ }
+ }
+ else{
+ if(errorR > errorL){
+ if(errorL > -5 && errorL < 5)
+ error = 0;
+ else
+ error = errorL;
+ }
+ else{
+ if(errorR > -5 && errorR < 5)
+ error = 0;
+ else
+ error = errorR;
+ }
+ }
+ }
+ return 0.069 + Kp*error;
+}
+
+int PID::getCenter(int centerL, int centerR) {
+ //turn right 122~64 122
+ //turn left 64~6 8
+
+ int result = 64;
+ int errorR = 64 - centerR;
+ int errorL = 64 - centerL;
+ int negL = 1 , negR = 1;
+
+
+
+
+ if(centerL == 128 && centerR == 0){// no black line
+
+ result = 64;
+
+ } else if(centerL == 128 && centerR != 0){// no left line
+
+ result = centerR;
+
+
+ } else if(centerL != 128 && centerR == 0){// no right line
+
+ result = centerL;
+
+ } else{// two black lines
+
+ negL = (errorL >= 0) ? 1 : -1;
+ negR = (errorR >= 0) ? 1 : -1;
+ errorL *= negL;
+ errorR *= negR;
+
+ result = (errorL >= errorR) ? centerL : centerR;
+ }
}
float PID::getInMin() {
diff -r 1464a3f0a290 -r d6d4e8adf7fe controller.h
--- a/controller.h Tue Jul 01 13:09:06 2014 +0000
+++ b/controller.h Wed Jul 02 13:33:49 2014 +0000
@@ -71,7 +71,8 @@
* PID calculation.
* @return The controller output as a float between outMin and outMax.
*/
- float compute(float center, float sp);
+ float compute(int centerL, int centerR, int sp);
+ int getCenter(int centerL, int centerR);
//Getters.
float getInMin();
diff -r 1464a3f0a290 -r d6d4e8adf7fe main.cpp
--- a/main.cpp Tue Jul 01 13:09:06 2014 +0000
+++ b/main.cpp Wed Jul 02 13:33:49 2014 +0000
@@ -7,8 +7,8 @@
#define Debug_cam_uart
-#define R_eye
-#define L_eye
+//#define R_eye
+//#define L_eye
#define motor_on
#define Pcontroller
#define task_ma_time
@@ -46,10 +46,11 @@
void run()
{
double motor, angle=0.0;
- int centerR,centerL;
+ int centerR,centerL,center;
double PID_v;
- double error , P, I, D, kp, r_kp;
- double last_error=0.0 ,last_I=0.0, last_brc, brc_df;
+ double P, I, D, kp = 0.0004, r_kp;
+ int errorR,errorL,error,last_error;
+ double last_I=0.0, last_brc, brc_df;
bool first_time=true, r_kp_neg=false;
while(1) {
@@ -61,7 +62,7 @@
#ifdef Debug_cam_uart
#ifdef L_eye
pc.printf("X");
- for(int i=122; i>=64; i--) {
+ for(int i=122; i>=44; i--) {
if(i==64)
pc.printf("X");
else
@@ -70,7 +71,7 @@
pc.printf(" || ");
#endif
#ifdef R_eye
- for(int i=64; i>=6; i--) {
+ for(int i=84; i>=6; i--) {
if(i==64)
pc.printf("X");
else
@@ -90,27 +91,22 @@
centerR=cam.black_centerR();
centerL=cam.black_centerL();
- if(centerR == 0 && centerL ==128){
- servo.set_angle(0.073);
- }
- else if(centerR == 0 && centerL !=128){//turn right
- PID_v=cam_to_M_ctrlr.compute(centerL,120);//set_angle()
- servo.set_angle(PID_v);//122~64
- }
- else if(centerR != 0 && centerL ==128){//turn left
- PID_v=cam_to_M_ctrlr.compute(centerR,8);//set_angle()
- servo.set_angle(PID_v);//64~6 64
+ center = cam_to_M_ctrlr.getCenter(centerL,centerR);
+
+ PID_v = (double)cam_to_M_ctrlr.compute(centerL,centerR,64);
+ //servo.set_angle(PID_v);
+ servo.set_angle(PID_v);
+ pc.printf("centerL: %d centerR: %d center: %d",centerL,centerR,center);
+ pc.printf("\r\n");
+ for(int i=118; i>=10; i--){
+ if(i==64)
+ pc.printf("X");
+ else if(i>=center-4 && i<=center+4)
+ pc.printf(" ");
+ else
+ pc.printf("O");
}
- else{
- PID_v=cam_to_M_ctrlr.compute((centerR+centerL)/2,64);//set_angle()
- servo.set_angle(PID_v);//64~6 64
- }
- pc.printf("centerL: %d centerR: %d",centerL,centerR);
pc.printf("\r\n");
- //pc.printf("angle: %f\r\n",PID_v);
- //pc.printf("%f %d %d speed :%f bk_center %f PID:%f \r\n",cam_to_M_ctrlr.de_v,cam.de_v,cam.de_v2,motor,center,PID_v);
- //center range 0~128
- last_error = error;
#ifdef task_ma_time
task_pin=0;
#endif