Clark Lin / Mbed 2 deprecated BX-car_s

QQQ

Dependencies:   mbed-rtos mbed

Fork of BX-car_s by Tony Lin

main.cpp@10:9f0ce6ba7663, 2014-06-26 (annotated)

Committer:
TonyLin
Date:
Thu Jun 26 14:29:53 2014 +0000
Revision:
10:9f0ce6ba7663
Parent:
9:33b99cb45e99 
ok;

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"
backman 7:fd976e1ced33 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 10:9f0ce6ba7663 14 #define offset 65
TonyLin 10:9f0ce6ba7663 15 #define kp 1
TonyLin 10:9f0ce6ba7663 16 #define ki 0.1
TonyLin 10:9f0ce6ba7663 17 #define kd 1
backman 9:33b99cb45e99 18 #include "TSISensor.h"
backman 7:fd976e1ced33 19
backman 6:5a39bde2e016 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 10:9f0ce6ba7663 25 BX_pot pot1('1'); // 90/30=3
TonyLin 10:9f0ce6ba7663 26 BX_pot pot2('2');
backman 9:33b99cb45e99 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;
TonyLin 10:9f0ce6ba7663 30
TonyLin 10:9f0ce6ba7663 31 double Computes(double, double, double);
TonyLin 10:9f0ce6ba7663 32 void run();
TonyLin 10:9f0ce6ba7663 33
TonyLin 10:9f0ce6ba7663 34 int main(){
TonyLin 10:9f0ce6ba7663 35 pc.baud(115200);
backman 0:68c173249c01 36
TonyLin 10:9f0ce6ba7663 37 while(1){
TonyLin 10:9f0ce6ba7663 38 if(tsi.readPercentage()>0.00011)
TonyLin 10:9f0ce6ba7663 39 break;
TonyLin 10:9f0ce6ba7663 40 }
TonyLin 10:9f0ce6ba7663 41 run();
backman 9:33b99cb45e99 42
TonyLin 10:9f0ce6ba7663 43 return 0;
TonyLin 10:9f0ce6ba7663 44 }
TonyLin 10:9f0ce6ba7663 45
TonyLin 10:9f0ce6ba7663 46 double Computes(double b_r_c, double* last_I, double* last_error){
TonyLin 10:9f0ce6ba7663 47 //stand at 100
TonyLin 10:9f0ce6ba7663 48 double error , P, I, D, PID;
backman 9:33b99cb45e99 49
TonyLin 10:9f0ce6ba7663 50 error=b_r_c-100;
TonyLin 10:9f0ce6ba7663 51 P=kp*error;
TonyLin 10:9f0ce6ba7663 52
TonyLin 10:9f0ce6ba7663 53 I=*last_I*2.0/3.0+error;
TonyLin 10:9f0ce6ba7663 54 *last_I=I;
TonyLin 10:9f0ce6ba7663 55 I=ki*I;
TonyLin 10:9f0ce6ba7663 56
TonyLin 10:9f0ce6ba7663 57 D=error-*last_error+error;
TonyLin 10:9f0ce6ba7663 58 *last_error=error;
TonyLin 10:9f0ce6ba7663 59 D=kd*D;
TonyLin 10:9f0ce6ba7663 60
TonyLin 10:9f0ce6ba7663 61 PID=P+I+D;
backman 9:33b99cb45e99 62
TonyLin 10:9f0ce6ba7663 63 if(PID<0.0){
TonyLin 10:9f0ce6ba7663 64 PID*=-1;
TonyLin 10:9f0ce6ba7663 65 PID=0.085-(PID*0.025);
TonyLin 10:9f0ce6ba7663 66 }
TonyLin 10:9f0ce6ba7663 67 else if(PID>0.0)
TonyLin 10:9f0ce6ba7663 68 PID=0.085+(PID*0.025);
TonyLin 10:9f0ce6ba7663 69 else
TonyLin 10:9f0ce6ba7663 70 PID=0.085;
TonyLin 10:9f0ce6ba7663 71
TonyLin 10:9f0ce6ba7663 72 return PID;
TonyLin 10:9f0ce6ba7663 73 }
TonyLin 10:9f0ce6ba7663 74
TonyLin 10:9f0ce6ba7663 75 void run(){
TonyLin 10:9f0ce6ba7663 76 double motor;
backman 8:8e49e21d80a2 77 double b_r_c;
backman 8:8e49e21d80a2 78 double PID_v;
backman 6:5a39bde2e016 79
TonyLin 10:9f0ce6ba7663 80 while(1){
TonyLin 10:9f0ce6ba7663 81 #ifdef task_ma_time
TonyLin 10:9f0ce6ba7663 82 task_pin=1;
TonyLin 10:9f0ce6ba7663 83 #endif
TonyLin 10:9f0ce6ba7663 84 cam.read();
TonyLin 10:9f0ce6ba7663 85 #ifdef Debug_cam_uart
TonyLin 10:9f0ce6ba7663 86 #ifdef L_eye
TonyLin 10:9f0ce6ba7663 87 for(int i=0;i<128;i++){
TonyLin 10:9f0ce6ba7663 88 if(i==64)
TonyLin 10:9f0ce6ba7663 89 pc.printf("X");
TonyLin 10:9f0ce6ba7663 90 else
TonyLin 10:9f0ce6ba7663 91 pc.printf("%c", cam.sign_line_imageL[i]);
TonyLin 10:9f0ce6ba7663 92 }
TonyLin 10:9f0ce6ba7663 93 pc.printf(" || ");
TonyLin 10:9f0ce6ba7663 94 #endif
TonyLin 10:9f0ce6ba7663 95 #ifdef R_eye
TonyLin 10:9f0ce6ba7663 96 for(int i=128;i>=0;i--){
TonyLin 10:9f0ce6ba7663 97 if(i==64)
TonyLin 10:9f0ce6ba7663 98 pc.printf("X");
TonyLin 10:9f0ce6ba7663 99 else if(i<10)
TonyLin 10:9f0ce6ba7663 100 pc.printf("-");
TonyLin 10:9f0ce6ba7663 101 else if(i>117)
TonyLin 10:9f0ce6ba7663 102 pc.printf("-");
TonyLin 10:9f0ce6ba7663 103 else
TonyLin 10:9f0ce6ba7663 104 pc.printf("%c", cam.sign_line_imageR[i]);
TonyLin 10:9f0ce6ba7663 105 }
TonyLin 10:9f0ce6ba7663 106 pc.printf("\r\n");
TonyLin 10:9f0ce6ba7663 107 #endif
TonyLin 10:9f0ce6ba7663 108 #endif
backman 3:c5f2281b3ed2 109
TonyLin 10:9f0ce6ba7663 110 #ifdef motor_on
TonyLin 10:9f0ce6ba7663 111 motor=pot1.read();
TonyLin 10:9f0ce6ba7663 112 MotorA.rotate(motor);
TonyLin 10:9f0ce6ba7663 113 MotorB.rotate(motor);
backman 7:fd976e1ced33 114 #endif
backman 7:fd976e1ced33 115
TonyLin 10:9f0ce6ba7663 116 b_r_c=(double)cam.black_centerR();
backman 8:8e49e21d80a2 117
TonyLin 10:9f0ce6ba7663 118 //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
TonyLin 10:9f0ce6ba7663 119 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,b_r_c,PID_v);
backman 8:8e49e21d80a2 120
TonyLin 10:9f0ce6ba7663 121 //servo.set_angle(PID_v);
TonyLin 10:9f0ce6ba7663 122
backman 7:fd976e1ced33 123
backman 8:8e49e21d80a2 124
TonyLin 10:9f0ce6ba7663 125 //PID_v = Computes(b_r_c, &last_I, &last_error);
TonyLin 10:9f0ce6ba7663 126 //compute
TonyLin 10:9f0ce6ba7663 127 //stand at 100
TonyLin 10:9f0ce6ba7663 128 double error , P, I, D;
TonyLin 10:9f0ce6ba7663 129 double last_error=0.0, last_I=0.0;
backman 7:fd976e1ced33 130
TonyLin 10:9f0ce6ba7663 131 if(error<20.0 && error>0.0)
TonyLin 10:9f0ce6ba7663 132 PID_v=0.0;
TonyLin 10:9f0ce6ba7663 133 else{
TonyLin 10:9f0ce6ba7663 134 error=b_r_c-offset;
TonyLin 10:9f0ce6ba7663 135 P=kp*error;
TonyLin 10:9f0ce6ba7663 136
TonyLin 10:9f0ce6ba7663 137 I=last_I*2.0/3.0+error;
TonyLin 10:9f0ce6ba7663 138 last_I=I;
TonyLin 10:9f0ce6ba7663 139 I=ki*I;
TonyLin 10:9f0ce6ba7663 140
TonyLin 10:9f0ce6ba7663 141 D=error-last_error;
TonyLin 10:9f0ce6ba7663 142 last_error=error;
TonyLin 10:9f0ce6ba7663 143 D=kd*D;
TonyLin 10:9f0ce6ba7663 144
TonyLin 10:9f0ce6ba7663 145 PID_v=P+I+D;
TonyLin 10:9f0ce6ba7663 146 }
TonyLin 10:9f0ce6ba7663 147
TonyLin 10:9f0ce6ba7663 148 if(PID_v < 0.0){
TonyLin 10:9f0ce6ba7663 149 PID_v*=-1;
TonyLin 10:9f0ce6ba7663 150 PID_v=0.085-(PID_v/125*0.025);
TonyLin 10:9f0ce6ba7663 151 }
TonyLin 10:9f0ce6ba7663 152 else if(PID_v > 0.0)
TonyLin 10:9f0ce6ba7663 153 PID_v=0.085+(PID_v/125*0.025);
TonyLin 10:9f0ce6ba7663 154 else
TonyLin 10:9f0ce6ba7663 155 PID_v=0.085;
TonyLin 10:9f0ce6ba7663 156
TonyLin 10:9f0ce6ba7663 157 servo.set_angle(PID_v);
TonyLin 10:9f0ce6ba7663 158
TonyLin 10:9f0ce6ba7663 159 #ifdef task_ma_time
TonyLin 10:9f0ce6ba7663 160 task_pin=0;
TonyLin 10:9f0ce6ba7663 161 #endif
TonyLin 10:9f0ce6ba7663 162 }
TonyLin 10:9f0ce6ba7663 163 }