Tony Lin / Mbed 2 deprecated BX-car_2

Dynamic kp!!!

Dependencies:   mbed-rtos mbed

Fork of BX-car by Tony Lin

main.cpp@13:a33a7705fe2b, 2014-06-28 (annotated)

Committer:
TonyLin
Date:
Sat Jun 28 07:06:54 2014 +0000
Revision:
13:a33a7705fe2b
Parent:
12:418e39749f48
Child:
14:303b22b76d7a
hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
backman 0:68c173249c01 1 #include "mbed.h"
backman 7:fd976e1ced33 2 #include "controller.h"
backman 1:82bc25a7b68b 3 #include "servo_api.h"
backman 1:82bc25a7b68b 4 #include "camera_api.h"
backman 6:5a39bde2e016 5 #include "motor_api.h"
backman 7:fd976e1ced33 6 #include "pot.h"
TonyLin 13:a33a7705fe2b 7
backman 3:c5f2281b3ed2 8
backman 3:c5f2281b3ed2 9 #define Debug_cam_uart
backman 6:5a39bde2e016 10 #define R_eye
backman 7:fd976e1ced33 11 #define motor_on
backman 6:5a39bde2e016 12 #define Pcontroller
backman 8:8e49e21d80a2 13 #define task_ma_time
TonyLin 13:a33a7705fe2b 14 #define offset 65
TonyLin 13:a33a7705fe2b 15 #define kp 1
TonyLin 13:a33a7705fe2b 16 #define ki 0.1
TonyLin 13:a33a7705fe2b 17 #define kd 1
TonyLin 13:a33a7705fe2b 18 #include "TSISensor.h"
backman 12:418e39749f48 19
TonyLin 13:a33a7705fe2b 20 Serial pc(USBTX, USBRX);
backman 1:82bc25a7b68b 21 BX_servo servo;
backman 6:5a39bde2e016 22 BX_camera cam;
backman 6:5a39bde2e016 23 BX_motor MotorA('A');
backman 6:5a39bde2e016 24 BX_motor MotorB('B');
TonyLin 13:a33a7705fe2b 25 BX_pot pot1('1'); // 90/30=3
TonyLin 13:a33a7705fe2b 26 BX_pot pot2('2');
TonyLin 13:a33a7705fe2b 27 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
backman 8:8e49e21d80a2 28 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 29 TSISensor tsi;
backman 12:418e39749f48 30
TonyLin 13:a33a7705fe2b 31 void run();
backman 12:418e39749f48 32
TonyLin 13:a33a7705fe2b 33 int main(){
TonyLin 13:a33a7705fe2b 34 pc.baud(115200);
backman 9:33b99cb45e99 35
TonyLin 13:a33a7705fe2b 36 while(1){
TonyLin 13:a33a7705fe2b 37 if(tsi.readPercentage()>0.00011)
TonyLin 13:a33a7705fe2b 38 break;
backman 12:418e39749f48 39 }
TonyLin 13:a33a7705fe2b 40 run();
TonyLin 13:a33a7705fe2b 41
TonyLin 13:a33a7705fe2b 42 pc.baud(115200);
TonyLin 13:a33a7705fe2b 43 return 0;
TonyLin 13:a33a7705fe2b 44 }
backman 12:418e39749f48 45
backman 12:418e39749f48 46
backman 7:fd976e1ced33 47
TonyLin 13:a33a7705fe2b 48 void run(){
TonyLin 13:a33a7705fe2b 49 double motor;
TonyLin 13:a33a7705fe2b 50 double b_r_c;
TonyLin 13:a33a7705fe2b 51 double PID_v;
TonyLin 13:a33a7705fe2b 52
TonyLin 13:a33a7705fe2b 53 while(1){
TonyLin 13:a33a7705fe2b 54 #ifdef task_ma_time
TonyLin 13:a33a7705fe2b 55 task_pin=1;
TonyLin 13:a33a7705fe2b 56 #endif
backman 12:418e39749f48 57
TonyLin 13:a33a7705fe2b 58 cam.read();
TonyLin 13:a33a7705fe2b 59 #ifdef Debug_cam_uart
TonyLin 13:a33a7705fe2b 60 #ifdef L_eye
TonyLin 13:a33a7705fe2b 61 for(int i=0;i<128;i++){
TonyLin 13:a33a7705fe2b 62 if(i==64)
TonyLin 13:a33a7705fe2b 63 pc.printf("X");
TonyLin 13:a33a7705fe2b 64 else
TonyLin 13:a33a7705fe2b 65 pc.printf("%c", cam.sign_line_imageL[i]);
TonyLin 13:a33a7705fe2b 66 }
TonyLin 13:a33a7705fe2b 67 pc.printf(" || ");
TonyLin 13:a33a7705fe2b 68 #endif
TonyLin 13:a33a7705fe2b 69 #ifdef R_eye
TonyLin 13:a33a7705fe2b 70 for(int i=128;i>=0;i--){
TonyLin 13:a33a7705fe2b 71 if(i==64)
TonyLin 13:a33a7705fe2b 72 pc.printf("X");
TonyLin 13:a33a7705fe2b 73 else if(i<10)
TonyLin 13:a33a7705fe2b 74 pc.printf("-");
TonyLin 13:a33a7705fe2b 75 else if(i>117)
TonyLin 13:a33a7705fe2b 76 pc.printf("-");
TonyLin 13:a33a7705fe2b 77 else
TonyLin 13:a33a7705fe2b 78 pc.printf("%c", cam.sign_line_imageR[i]);
TonyLin 13:a33a7705fe2b 79 }
TonyLin 13:a33a7705fe2b 80 pc.printf("\r\n");
TonyLin 13:a33a7705fe2b 81 #endif
TonyLin 13:a33a7705fe2b 82 #endif
TonyLin 13:a33a7705fe2b 83
TonyLin 13:a33a7705fe2b 84 #ifdef motor_on
TonyLin 13:a33a7705fe2b 85 motor=pot1.read();
TonyLin 13:a33a7705fe2b 86 MotorA.rotate(motor);
TonyLin 13:a33a7705fe2b 87 MotorB.rotate(motor);
TonyLin 13:a33a7705fe2b 88 #endif
TonyLin 13:a33a7705fe2b 89
TonyLin 13:a33a7705fe2b 90 b_r_c=(double)cam.black_centerR();
TonyLin 13:a33a7705fe2b 91 pc.printf("%d %d speed :%f bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,motor,b_r_c,PID_v);
TonyLin 13:a33a7705fe2b 92
TonyLin 13:a33a7705fe2b 93 //compute
TonyLin 13:a33a7705fe2b 94 //stand at 100
TonyLin 13:a33a7705fe2b 95 double error , P, I, D;
TonyLin 13:a33a7705fe2b 96 double last_error=0.0, last_I=0.0;
TonyLin 13:a33a7705fe2b 97
TonyLin 13:a33a7705fe2b 98 if(error<20.0 && error>0.0)
TonyLin 13:a33a7705fe2b 99 PID_v=0.0;
TonyLin 13:a33a7705fe2b 100 else{
TonyLin 13:a33a7705fe2b 101 error=b_r_c-offset;
TonyLin 13:a33a7705fe2b 102 P=kp*error;
TonyLin 13:a33a7705fe2b 103
TonyLin 13:a33a7705fe2b 104 I=last_I*2.0/3.0+error;
TonyLin 13:a33a7705fe2b 105 last_I=I;
TonyLin 13:a33a7705fe2b 106 I=ki*I;
backman 12:418e39749f48 107
TonyLin 13:a33a7705fe2b 108 D=error-last_error;
TonyLin 13:a33a7705fe2b 109 last_error=error;
TonyLin 13:a33a7705fe2b 110 D=kd*D;
backman 7:fd976e1ced33 111
TonyLin 13:a33a7705fe2b 112 PID_v=P+I+D;
backman 12:418e39749f48 113 }
TonyLin 13:a33a7705fe2b 114
TonyLin 13:a33a7705fe2b 115 if(PID_v < 0.0){
TonyLin 13:a33a7705fe2b 116 PID_v*=-1;
TonyLin 13:a33a7705fe2b 117 PID_v=0.085-(PID_v/125*0.025);
TonyLin 13:a33a7705fe2b 118 }
TonyLin 13:a33a7705fe2b 119 else if(PID_v > 0.0)
TonyLin 13:a33a7705fe2b 120 PID_v=0.085+(PID_v/125*0.025);
TonyLin 13:a33a7705fe2b 121 else
TonyLin 13:a33a7705fe2b 122 PID_v=0.085;
TonyLin 13:a33a7705fe2b 123
TonyLin 13:a33a7705fe2b 124 servo.set_angle(PID_v);
TonyLin 13:a33a7705fe2b 125
TonyLin 13:a33a7705fe2b 126 #ifdef task_ma_time
TonyLin 13:a33a7705fe2b 127 task_pin=0;
TonyLin 13:a33a7705fe2b 128 #endif
backman 7:fd976e1ced33 129 }
TonyLin 13:a33a7705fe2b 130 }