kao yi / Mbed 2 deprecated Boboobooov5

wu

Dependencies:   mbed-rtos mbed

Fork of Boboobooov4 by kao yi

main.cpp@8:8e49e21d80a2, 2014-06-22 (annotated)

Committer:
backman
Date:
Sun Jun 22 15:29:20 2014 +0000
Revision:
8:8e49e21d80a2
Parent:
7:fd976e1ced33
Child:
9:33b99cb45e99
same work

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