譯文 張 / Mbed 2 deprecated BX-car

good

Dependencies:   mbed

Fork of BX-car by Clark Lin

main.cpp@10:d2401a243e8d, 2014-06-25 (annotated)

Committer:
physicsgood
Date:
Wed Jun 25 05:07:13 2014 +0000
Revision:
10:d2401a243e8d
Parent:
9:33b99cb45e99
Child:
11:ffd762ae141b
for even

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
backman 9:33b99cb45e99 14 #include "TSISensor.h"
backman 7:fd976e1ced33 15
backman 6:5a39bde2e016 16 Serial pc(USBTX, USBRX);
backman 0:68c173249c01 17
backman 0:68c173249c01 18
backman 1:82bc25a7b68b 19 BX_servo servo;
backman 6:5a39bde2e016 20
backman 6:5a39bde2e016 21 BX_camera cam;
backman 0:68c173249c01 22
backman 6:5a39bde2e016 23 BX_motor MotorA('A');
backman 6:5a39bde2e016 24 BX_motor MotorB('B');
backman 3:c5f2281b3ed2 25
backman 7:fd976e1ced33 26 BX_pot pot1('1');
backman 7:fd976e1ced33 27
backman 9:33b99cb45e99 28 // 90/30=3
backman 9:33b99cb45e99 29 PID cam_to_M_ctrlr(10.0,118.0,0.06,0.11,(0.104/30),0.0,0.0,10);
physicsgood 10:d2401a243e8d 30 //PID(float in_min,float in_max,float out_min,float out_max,float Kc, float tauI, float tauD, float interval)
physicsgood 10:d2401a243e8d 31 //in_min in_out camera array
physicsgood 10:d2401a243e8d 32 //out_min out_max servo range
physicsgood 10:d2401a243e8d 33 //
backman 7:fd976e1ced33 34
backman 8:8e49e21d80a2 35 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 36 TSISensor tsi;
backman 0:68c173249c01 37 int main() {
backman 0:68c173249c01 38
backman 9:33b99cb45e99 39
backman 9:33b99cb45e99 40 pc.baud(115200);
backman 9:33b99cb45e99 41 while(1){
backman 9:33b99cb45e99 42
backman 9:33b99cb45e99 43 if(tsi.readPercentage()>0.00011)
backman 9:33b99cb45e99 44 break;
backman 9:33b99cb45e99 45 }
backman 1:82bc25a7b68b 46
backman 9:33b99cb45e99 47
backman 6:5a39bde2e016 48
backman 1:82bc25a7b68b 49
backman 7:fd976e1ced33 50 double motor;
backman 8:8e49e21d80a2 51 double b_r_c;
backman 8:8e49e21d80a2 52 double PID_v;
backman 6:5a39bde2e016 53 while(1){
backman 1:82bc25a7b68b 54
backman 7:fd976e1ced33 55 #ifdef task_ma_time
backman 7:fd976e1ced33 56 task_pin=1;
backman 7:fd976e1ced33 57 #endif
backman 7:fd976e1ced33 58
backman 7:fd976e1ced33 59
backman 7:fd976e1ced33 60
backman 6:5a39bde2e016 61 cam.read();
backman 6:5a39bde2e016 62
backman 7:fd976e1ced33 63 #ifdef Debug_cam_uart
backman 7:fd976e1ced33 64
backman 6:5a39bde2e016 65 #ifdef L_eye
backman 1:82bc25a7b68b 66 for(int i=0;i<128;i++){
backman 7:fd976e1ced33 67
backman 7:fd976e1ced33 68
backman 7:fd976e1ced33 69
backman 7:fd976e1ced33 70
backman 1:82bc25a7b68b 71 if(i==64)
backman 1:82bc25a7b68b 72 pc.printf("X");
backman 1:82bc25a7b68b 73 else
backman 2:c51647d3c14d 74 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 75 }
backman 2:c51647d3c14d 76 pc.printf(" || ");
backman 7:fd976e1ced33 77 #endif
backman 7:fd976e1ced33 78 #ifdef R_eye
backman 7:fd976e1ced33 79 for(int i=128;i>=0;i--){
backman 2:c51647d3c14d 80 if(i==64)
backman 2:c51647d3c14d 81 pc.printf("X");
backman 7:fd976e1ced33 82
backman 7:fd976e1ced33 83 else if(i<10)
backman 7:fd976e1ced33 84 pc.printf("-");
backman 7:fd976e1ced33 85
backman 7:fd976e1ced33 86 else if(i>117)
backman 7:fd976e1ced33 87
backman 7:fd976e1ced33 88
backman 7:fd976e1ced33 89 pc.printf("-");
backman 7:fd976e1ced33 90
backman 2:c51647d3c14d 91 else
backman 2:c51647d3c14d 92 pc.printf("%c", cam.sign_line_imageR[i]);
backman 7:fd976e1ced33 93
backman 7:fd976e1ced33 94
backman 7:fd976e1ced33 95
backman 7:fd976e1ced33 96
backman 7:fd976e1ced33 97
backman 7:fd976e1ced33 98
backman 1:82bc25a7b68b 99 }
backman 1:82bc25a7b68b 100 pc.printf("\r\n");
backman 6:5a39bde2e016 101
backman 6:5a39bde2e016 102
backman 6:5a39bde2e016 103
backman 3:c5f2281b3ed2 104
backman 7:fd976e1ced33 105 #endif
backman 7:fd976e1ced33 106
backman 7:fd976e1ced33 107
backman 3:c5f2281b3ed2 108
backman 7:fd976e1ced33 109
backman 3:c5f2281b3ed2 110
backman 3:c5f2281b3ed2 111
backman 6:5a39bde2e016 112
backman 3:c5f2281b3ed2 113
backman 6:5a39bde2e016 114
backman 2:c51647d3c14d 115
backman 7:fd976e1ced33 116 #endif
backman 1:82bc25a7b68b 117
backman 7:fd976e1ced33 118
backman 1:82bc25a7b68b 119
backman 1:82bc25a7b68b 120
backman 1:82bc25a7b68b 121
backman 1:82bc25a7b68b 122
backman 1:82bc25a7b68b 123
backman 6:5a39bde2e016 124
backman 6:5a39bde2e016 125
backman 7:fd976e1ced33 126
backman 7:fd976e1ced33 127 #ifdef motor_on
backman 7:fd976e1ced33 128
backman 7:fd976e1ced33 129
backman 7:fd976e1ced33 130 motor=pot1.read();
backman 6:5a39bde2e016 131
backman 7:fd976e1ced33 132
backman 1:82bc25a7b68b 133
backman 7:fd976e1ced33 134
backman 7:fd976e1ced33 135 MotorA.rotate(motor);
backman 7:fd976e1ced33 136 MotorB.rotate(motor);
backman 7:fd976e1ced33 137 #endif
backman 7:fd976e1ced33 138
backman 7:fd976e1ced33 139
backman 7:fd976e1ced33 140
backman 7:fd976e1ced33 141
backman 7:fd976e1ced33 142
backman 8:8e49e21d80a2 143 b_r_c=(double)cam.black_centerR();
backman 8:8e49e21d80a2 144
physicsgood 10:d2401a243e8d 145 //PID_v=cam_to_M_ctrlr.compute(b_r_c,15.0);
physicsgood 10:d2401a243e8d 146 //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);
physicsgood 10:d2401a243e8d 147 pc.printf("b_r_c %lf\r\n",b_r_c);
physicsgood 10:d2401a243e8d 148 int error = b_r_c-64;
physicsgood 10:d2401a243e8d 149 if(error<8&&error>-8){
physicsgood 10:d2401a243e8d 150 servo.set_angle(0.085);
physicsgood 10:d2401a243e8d 151 }
physicsgood 10:d2401a243e8d 152 else if(error<-8){
physicsgood 10:d2401a243e8d 153 servo.set_angle(0.1);
physicsgood 10:d2401a243e8d 154 }
physicsgood 10:d2401a243e8d 155 else if(error>8){
physicsgood 10:d2401a243e8d 156 servo.set_angle(0.07);
physicsgood 10:d2401a243e8d 157 }
backman 8:8e49e21d80a2 158
physicsgood 10:d2401a243e8d 159 //servo.set_angle(PID_v);
backman 7:fd976e1ced33 160
backman 8:8e49e21d80a2 161
backman 7:fd976e1ced33 162
backman 7:fd976e1ced33 163
backman 7:fd976e1ced33 164 #ifdef task_ma_time
backman 7:fd976e1ced33 165 task_pin=0;
backman 7:fd976e1ced33 166 #endif
backman 7:fd976e1ced33 167
backman 7:fd976e1ced33 168 }
backman 7:fd976e1ced33 169
backman 6:5a39bde2e016 170
backman 6:5a39bde2e016 171
backman 7:fd976e1ced33 172
backman 0:68c173249c01 173
backman 0:68c173249c01 174
backman 0:68c173249c01 175
backman 1:82bc25a7b68b 176 return 0;
backman 0:68c173249c01 177
backman 0:68c173249c01 178
backman 0:68c173249c01 179 }