kao yi / Mbed 2 deprecated BX-car

running car

Dependencies:   mbed mbed-rtos

Fork of Boboobooo by kao yi

main.cpp@10:03d5aa2511c4, 2014-06-26 (annotated)

Committer:
backman
Date:
Thu Jun 26 09:15:35 2014 +0000
Revision:
10:03d5aa2511c4
Parent:
9:33b99cb45e99
Child:
11:418e39749f48
without os

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 10:03d5aa2511c4 29 PID cam_to_M_ctrlr(10.0,118.0,0.059,0.105,0.105-0.059,0.00,0.02,10);
backman 7:fd976e1ced33 30
backman 8:8e49e21d80a2 31 DigitalOut task_pin(PTD1);
backman 9:33b99cb45e99 32 TSISensor tsi;
backman 0:68c173249c01 33 int main() {
backman 0:68c173249c01 34
backman 9:33b99cb45e99 35
backman 9:33b99cb45e99 36 pc.baud(115200);
backman 9:33b99cb45e99 37
backman 9:33b99cb45e99 38 while(1){
backman 9:33b99cb45e99 39
backman 9:33b99cb45e99 40 if(tsi.readPercentage()>0.00011)
backman 9:33b99cb45e99 41 break;
backman 9:33b99cb45e99 42 }
backman 1:82bc25a7b68b 43
backman 9:33b99cb45e99 44
backman 6:5a39bde2e016 45
backman 1:82bc25a7b68b 46
backman 7:fd976e1ced33 47 double motor;
backman 8:8e49e21d80a2 48 double b_r_c;
backman 8:8e49e21d80a2 49 double PID_v;
backman 6:5a39bde2e016 50 while(1){
backman 1:82bc25a7b68b 51
backman 7:fd976e1ced33 52 #ifdef task_ma_time
backman 7:fd976e1ced33 53 task_pin=1;
backman 7:fd976e1ced33 54 #endif
backman 7:fd976e1ced33 55
backman 7:fd976e1ced33 56
backman 7:fd976e1ced33 57
backman 6:5a39bde2e016 58 cam.read();
backman 6:5a39bde2e016 59
backman 7:fd976e1ced33 60 #ifdef Debug_cam_uart
backman 7:fd976e1ced33 61
backman 6:5a39bde2e016 62 #ifdef L_eye
backman 1:82bc25a7b68b 63 for(int i=0;i<128;i++){
backman 7:fd976e1ced33 64
backman 7:fd976e1ced33 65
backman 7:fd976e1ced33 66
backman 7:fd976e1ced33 67
backman 1:82bc25a7b68b 68 if(i==64)
backman 1:82bc25a7b68b 69 pc.printf("X");
backman 1:82bc25a7b68b 70 else
backman 2:c51647d3c14d 71 pc.printf("%c", cam.sign_line_imageL[i]);
backman 2:c51647d3c14d 72 }
backman 2:c51647d3c14d 73 pc.printf(" || ");
backman 7:fd976e1ced33 74 #endif
backman 7:fd976e1ced33 75 #ifdef R_eye
backman 7:fd976e1ced33 76 for(int i=128;i>=0;i--){
backman 2:c51647d3c14d 77 if(i==64)
backman 2:c51647d3c14d 78 pc.printf("X");
backman 7:fd976e1ced33 79
backman 7:fd976e1ced33 80 else if(i<10)
backman 7:fd976e1ced33 81 pc.printf("-");
backman 7:fd976e1ced33 82
backman 7:fd976e1ced33 83 else if(i>117)
backman 7:fd976e1ced33 84
backman 7:fd976e1ced33 85
backman 7:fd976e1ced33 86 pc.printf("-");
backman 7:fd976e1ced33 87
backman 2:c51647d3c14d 88 else
backman 2:c51647d3c14d 89 pc.printf("%c", cam.sign_line_imageR[i]);
backman 7:fd976e1ced33 90
backman 7:fd976e1ced33 91
backman 7:fd976e1ced33 92
backman 7:fd976e1ced33 93
backman 7:fd976e1ced33 94
backman 7:fd976e1ced33 95
backman 1:82bc25a7b68b 96 }
backman 1:82bc25a7b68b 97 pc.printf("\r\n");
backman 6:5a39bde2e016 98
backman 6:5a39bde2e016 99
backman 6:5a39bde2e016 100
backman 3:c5f2281b3ed2 101
backman 7:fd976e1ced33 102 #endif
backman 7:fd976e1ced33 103
backman 7:fd976e1ced33 104
backman 3:c5f2281b3ed2 105
backman 7:fd976e1ced33 106
backman 3:c5f2281b3ed2 107
backman 3:c5f2281b3ed2 108
backman 6:5a39bde2e016 109
backman 3:c5f2281b3ed2 110
backman 6:5a39bde2e016 111
backman 2:c51647d3c14d 112
backman 7:fd976e1ced33 113 #endif
backman 1:82bc25a7b68b 114
backman 7:fd976e1ced33 115
backman 1:82bc25a7b68b 116
backman 1:82bc25a7b68b 117
backman 1:82bc25a7b68b 118
backman 1:82bc25a7b68b 119
backman 1:82bc25a7b68b 120
backman 6:5a39bde2e016 121
backman 6:5a39bde2e016 122
backman 7:fd976e1ced33 123
backman 7:fd976e1ced33 124 #ifdef motor_on
backman 7:fd976e1ced33 125
backman 7:fd976e1ced33 126
backman 7:fd976e1ced33 127 motor=pot1.read();
backman 6:5a39bde2e016 128
backman 7:fd976e1ced33 129
backman 1:82bc25a7b68b 130
backman 7:fd976e1ced33 131
backman 7:fd976e1ced33 132 MotorA.rotate(motor);
backman 7:fd976e1ced33 133 MotorB.rotate(motor);
backman 7:fd976e1ced33 134 #endif
backman 7:fd976e1ced33 135
backman 7:fd976e1ced33 136
backman 7:fd976e1ced33 137
backman 7:fd976e1ced33 138
backman 7:fd976e1ced33 139
backman 10:03d5aa2511c4 140 b_r_c=(double)cam.black_centerR();
backman 8:8e49e21d80a2 141
backman 10:03d5aa2511c4 142 PID_v=cam_to_M_ctrlr.compute(b_r_c,64.0);
backman 10:03d5aa2511c4 143
backman 8:8e49e21d80a2 144
backman 8:8e49e21d80a2 145
backman 10:03d5aa2511c4 146 #ifdef Debug_cam_uart
backman 10:03d5aa2511c4 147
backman 10:03d5aa2511c4 148 pc.printf("cam %d %d k:%f i:%f d:%f speed :%f bk_center %f PID:%f \r\n",cam.de_v,cam.de_v2,cam_to_M_ctrlr.de_kp,cam_to_M_ctrlr.de_ip,cam_to_M_ctrlr.de_dp,motor,b_r_c,PID_v);
backman 10:03d5aa2511c4 149
backman 10:03d5aa2511c4 150 #endif
backman 8:8e49e21d80a2 151 servo.set_angle(PID_v);
backman 7:fd976e1ced33 152
backman 10:03d5aa2511c4 153
backman 7:fd976e1ced33 154
backman 7:fd976e1ced33 155
backman 7:fd976e1ced33 156 #ifdef task_ma_time
backman 7:fd976e1ced33 157 task_pin=0;
backman 7:fd976e1ced33 158 #endif
backman 7:fd976e1ced33 159
backman 7:fd976e1ced33 160 }
backman 7:fd976e1ced33 161
backman 6:5a39bde2e016 162
backman 6:5a39bde2e016 163
backman 7:fd976e1ced33 164
backman 0:68c173249c01 165
backman 0:68c173249c01 166
backman 0:68c173249c01 167
backman 1:82bc25a7b68b 168 return 0;
backman 0:68c173249c01 169
backman 0:68c173249c01 170
backman 0:68c173249c01 171 }