Ryo Ogata / Mbed 2 deprecated jumpROBO4_0

miseru

Dependencies:   ColorSensor HMC6352 Servo TextLCD mbed

main.cpp@0:4c9a3b107572, 2013-09-12 (annotated)

Committer:
OGA
Date:
Thu Sep 12 12:10:38 2013 +0000
Revision:
0:4c9a3b107572
sensei

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:4c9a3b107572 1 #include "mbed.h"
OGA 0:4c9a3b107572 2 #include "ColorSensor.h"
OGA 0:4c9a3b107572 3 #include "TextLCD.h"
OGA 0:4c9a3b107572 4 #include "Servo.h"
OGA 0:4c9a3b107572 5 #include "HMC6352.h"
OGA 0:4c9a3b107572 6
OGA 0:4c9a3b107572 7 #include "main.h"
OGA 0:4c9a3b107572 8
OGA 0:4c9a3b107572 9 //センサの数
OGA 0:4c9a3b107572 10 #define COLOR_NUM 3
OGA 0:4c9a3b107572 11
OGA 0:4c9a3b107572 12 //閾値
OGA 0:4c9a3b107572 13 #define R_THR 65
OGA 0:4c9a3b107572 14 #define G_THR 65
OGA 0:4c9a3b107572 15 #define B_THR 65
OGA 0:4c9a3b107572 16 #define PINR_THR 1000
OGA 0:4c9a3b107572 17
OGA 0:4c9a3b107572 18 enum{
OGA 0:4c9a3b107572 19 GO,
OGA 0:4c9a3b107572 20 STOP
OGA 0:4c9a3b107572 21 };
OGA 0:4c9a3b107572 22
OGA 0:4c9a3b107572 23 //TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
OGA 0:4c9a3b107572 24
OGA 0:4c9a3b107572 25 ColorSensor color0(p20, p17, p18, p19, 5);
OGA 0:4c9a3b107572 26 ColorSensor color1(p16, p13, p14, p15, 5);
OGA 0:4c9a3b107572 27 ColorSensor color2(p12, p9, p10, p11, 5);
OGA 0:4c9a3b107572 28
OGA 0:4c9a3b107572 29 Servo servoR(p23);
OGA 0:4c9a3b107572 30 Servo servoL(p24);
OGA 0:4c9a3b107572 31
OGA 0:4c9a3b107572 32 //ColorSensor color = ColorSensor();
OGA 0:4c9a3b107572 33 //Serial pc(USBTX, USBRX); // tx, rx
OGA 0:4c9a3b107572 34
OGA 0:4c9a3b107572 35 DigitalOut led[4] = {LED1,LED2,LED3,LED4};
OGA 0:4c9a3b107572 36 DigitalOut air[2] = {p21,p22};
OGA 0:4c9a3b107572 37
OGA 0:4c9a3b107572 38 Timer color_t[3];
OGA 0:4c9a3b107572 39 Timer ping_t;
OGA 0:4c9a3b107572 40 Ticker interrupt0;
OGA 0:4c9a3b107572 41
OGA 0:4c9a3b107572 42 double proportional = 0;
OGA 0:4c9a3b107572 43 uint16_t com_val = 0;
OGA 0:4c9a3b107572 44 unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
OGA 0:4c9a3b107572 45
OGA 0:4c9a3b107572 46 #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
OGA 0:4c9a3b107572 47
OGA 0:4c9a3b107572 48 #define STRAIGHT 0.6;
OGA 0:4c9a3b107572 49 #define SPIN 0.4;
OGA 0:4c9a3b107572 50
OGA 0:4c9a3b107572 51 //#define STRAIGHT 0.0;
OGA 0:4c9a3b107572 52 //#define SPIN 1.0;
OGA 0:4c9a3b107572 53
OGA 0:4c9a3b107572 54
OGA 0:4c9a3b107572 55
OGA 0:4c9a3b107572 56 void tic_sensor()
OGA 0:4c9a3b107572 57 {
OGA 0:4c9a3b107572 58 Ultrasonic();
OGA 0:4c9a3b107572 59
OGA 0:4c9a3b107572 60 color0.checkRGB(redp[0],greenp[0],bluep[0]);
OGA 0:4c9a3b107572 61 color1.checkRGB(redp[1],greenp[1],bluep[1]);
OGA 0:4c9a3b107572 62 color2.checkRGB(redp[2],greenp[2],bluep[2]);
OGA 0:4c9a3b107572 63
OGA 0:4c9a3b107572 64 //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:4c9a3b107572 65 }
OGA 0:4c9a3b107572 66
OGA 0:4c9a3b107572 67
OGA 0:4c9a3b107572 68 void move(int vl,int vs){
OGA 0:4c9a3b107572 69 double fut_R,fut_L,true_vs;
OGA 0:4c9a3b107572 70
OGA 0:4c9a3b107572 71 true_vs = abs(vs)/SPIN;
OGA 0:4c9a3b107572 72
OGA 0:4c9a3b107572 73 if(true_vs > 40){
OGA 0:4c9a3b107572 74 vl = 0;
OGA 0:4c9a3b107572 75 vs = 100*(vs/abs(vs));
OGA 0:4c9a3b107572 76 }
OGA 0:4c9a3b107572 77
OGA 0:4c9a3b107572 78 fut_R = Convert_dekaruto((vl + vs));
OGA 0:4c9a3b107572 79 fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
OGA 0:4c9a3b107572 80
OGA 0:4c9a3b107572 81 fut_R = Convert_dekaruto(-50);
OGA 0:4c9a3b107572 82 fut_L = Convert_dekaruto(-50);
OGA 0:4c9a3b107572 83
OGA 0:4c9a3b107572 84 servoR = fut_R;
OGA 0:4c9a3b107572 85 servoL = fut_L;
OGA 0:4c9a3b107572 86
OGA 0:4c9a3b107572 87 //printf("R:%lf L:%lf\n",fut_R,fut_L);
OGA 0:4c9a3b107572 88 //printf("R:%d L:%d\n",(vl + vs),-(vl - vs));
OGA 0:4c9a3b107572 89 }
OGA 0:4c9a3b107572 90
OGA 0:4c9a3b107572 91 /*void PidUpdate()
OGA 0:4c9a3b107572 92 {
OGA 0:4c9a3b107572 93 inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
OGA 0:4c9a3b107572 94 //printf("%lf\n",inputPID);
OGA 0:4c9a3b107572 95 }*/
OGA 0:4c9a3b107572 96
OGA 0:4c9a3b107572 97 double vsOut(){
OGA 0:4c9a3b107572 98 double vs;
OGA 0:4c9a3b107572 99 vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
OGA 0:4c9a3b107572 100 vs = vs * 8;
OGA 0:4c9a3b107572 101
OGA 0:4c9a3b107572 102 if(vs/abs(vs) < 0){
OGA 0:4c9a3b107572 103 //vs *= 1.3;
OGA 0:4c9a3b107572 104 }
OGA 0:4c9a3b107572 105
OGA 0:4c9a3b107572 106 if(abs(vs) > 90)vs = 90*(vs/abs(vs));
OGA 0:4c9a3b107572 107 if(abs(vs) < 25) vs = 25*(vs/abs(vs));
OGA 0:4c9a3b107572 108
OGA 0:4c9a3b107572 109 return vs;
OGA 0:4c9a3b107572 110 }
OGA 0:4c9a3b107572 111
OGA 0:4c9a3b107572 112
OGA 0:4c9a3b107572 113 int ping_button(int ping,int button){
OGA 0:4c9a3b107572 114 static int continue_flag = 0;
OGA 0:4c9a3b107572 115 static int change_flag = 0;
OGA 0:4c9a3b107572 116
OGA 0:4c9a3b107572 117 if(continue_flag == 0){
OGA 0:4c9a3b107572 118 if(ping <= PINR_THR){
OGA 0:4c9a3b107572 119 ping_t.start();
OGA 0:4c9a3b107572 120 continue_flag = 1;
OGA 0:4c9a3b107572 121 }
OGA 0:4c9a3b107572 122 }
OGA 0:4c9a3b107572 123
OGA 0:4c9a3b107572 124 if(continue_flag == 1){
OGA 0:4c9a3b107572 125 //agatterutoki
OGA 0:4c9a3b107572 126 if(ping <= PINR_THR){
OGA 0:4c9a3b107572 127 if(change_flag == 0){
OGA 0:4c9a3b107572 128 if(ping_t.read_ms() >= 300){
OGA 0:4c9a3b107572 129 button = !button;
OGA 0:4c9a3b107572 130 change_flag = 1;
OGA 0:4c9a3b107572 131 }
OGA 0:4c9a3b107572 132 }
OGA 0:4c9a3b107572 133 }
OGA 0:4c9a3b107572 134 //tatisagari
OGA 0:4c9a3b107572 135 if(ping >= (PINR_THR+200)){
OGA 0:4c9a3b107572 136 ping_t.stop();
OGA 0:4c9a3b107572 137 ping_t.reset();
OGA 0:4c9a3b107572 138 continue_flag = 0;
OGA 0:4c9a3b107572 139 change_flag = 0;
OGA 0:4c9a3b107572 140 }
OGA 0:4c9a3b107572 141 }
OGA 0:4c9a3b107572 142 return button;
OGA 0:4c9a3b107572 143 }
OGA 0:4c9a3b107572 144
OGA 0:4c9a3b107572 145
OGA 0:4c9a3b107572 146 //青から赤に反応したらジャンプ
OGA 0:4c9a3b107572 147 //pidUpdataのせいでカラーセンサが動かない!!
OGA 0:4c9a3b107572 148 int main()
OGA 0:4c9a3b107572 149 {
OGA 0:4c9a3b107572 150 //wait(3);
OGA 0:4c9a3b107572 151
OGA 0:4c9a3b107572 152 timer2.start();
OGA 0:4c9a3b107572 153 ping_t.start();
OGA 0:4c9a3b107572 154
OGA 0:4c9a3b107572 155
OGA 0:4c9a3b107572 156 unsigned R, G, B;
OGA 0:4c9a3b107572 157 unsigned threshold, t[3] = {0} ;
OGA 0:4c9a3b107572 158 double color_sum;
OGA 0:4c9a3b107572 159 int vl;
OGA 0:4c9a3b107572 160 double vs;
OGA 0:4c9a3b107572 161 uint8_t button, state=0;
OGA 0:4c9a3b107572 162
OGA 0:4c9a3b107572 163 //pc.baud(115200);
OGA 0:4c9a3b107572 164 air[0] = 0; air[1] = 1;
OGA 0:4c9a3b107572 165
OGA 0:4c9a3b107572 166
OGA 0:4c9a3b107572 167 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
OGA 0:4c9a3b107572 168 //pidUpdata.attach(&PidUpdate, PID_CYCLE);
OGA 0:4c9a3b107572 169 interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
OGA 0:4c9a3b107572 170
OGA 0:4c9a3b107572 171 int e=0;
OGA 0:4c9a3b107572 172 while(1)
OGA 0:4c9a3b107572 173 {
OGA 0:4c9a3b107572 174
OGA 0:4c9a3b107572 175
OGA 0:4c9a3b107572 176 //lcd.cls();
OGA 0:4c9a3b107572 177 //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:4c9a3b107572 178 /*
OGA 0:4c9a3b107572 179 lcd.locate(0,0);
OGA 0:4c9a3b107572 180 lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
OGA 0:4c9a3b107572 181 lcd.locate(0,1);
OGA 0:4c9a3b107572 182 lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
OGA 0:4c9a3b107572 183 */
OGA 0:4c9a3b107572 184 pc.printf("%d\n", ultrasonicVal[0]);
OGA 0:4c9a3b107572 185
OGA 0:4c9a3b107572 186
OGA 0:4c9a3b107572 187 threshold = 0;
OGA 0:4c9a3b107572 188 for(int i=0; i<3; i++){
OGA 0:4c9a3b107572 189 if(bluep[i] >= B_THR){
OGA 0:4c9a3b107572 190 color_t[i].reset();
OGA 0:4c9a3b107572 191 color_t[i].start();
OGA 0:4c9a3b107572 192 t[i] = 0;
OGA 0:4c9a3b107572 193 }else if(redp[i] >= R_THR){
OGA 0:4c9a3b107572 194 t[i] = color_t[i].read_ms();
OGA 0:4c9a3b107572 195 }else{
OGA 0:4c9a3b107572 196 t[i] = 0;
OGA 0:4c9a3b107572 197 }
OGA 0:4c9a3b107572 198
OGA 0:4c9a3b107572 199 if((t[i] <= 500) && (t[i] != 0)){
OGA 0:4c9a3b107572 200 threshold++;
OGA 0:4c9a3b107572 201 }
OGA 0:4c9a3b107572 202 }
OGA 0:4c9a3b107572 203
OGA 0:4c9a3b107572 204
OGA 0:4c9a3b107572 205
OGA 0:4c9a3b107572 206 if(threshold >= 1){
OGA 0:4c9a3b107572 207 led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
OGA 0:4c9a3b107572 208 air[0] = 1; air[1] = 0;
OGA 0:4c9a3b107572 209 wait(0.4);
OGA 0:4c9a3b107572 210 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:4c9a3b107572 211 air[0] = 0; air[1] = 1;
OGA 0:4c9a3b107572 212 wait(0.5);
OGA 0:4c9a3b107572 213 }else{
OGA 0:4c9a3b107572 214 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:4c9a3b107572 215 }
OGA 0:4c9a3b107572 216
OGA 0:4c9a3b107572 217
OGA 0:4c9a3b107572 218
OGA 0:4c9a3b107572 219 button = ping_button(ultrasonicVal[0],button);
OGA 0:4c9a3b107572 220
OGA 0:4c9a3b107572 221 if(button){
OGA 0:4c9a3b107572 222 state = GO;
OGA 0:4c9a3b107572 223 }else{
OGA 0:4c9a3b107572 224 state = STOP;
OGA 0:4c9a3b107572 225 }
OGA 0:4c9a3b107572 226
OGA 0:4c9a3b107572 227
OGA 0:4c9a3b107572 228
OGA 0:4c9a3b107572 229
OGA 0:4c9a3b107572 230
OGA 0:4c9a3b107572 231 if(state == GO){
OGA 0:4c9a3b107572 232 vl = -90;//led[0] = 1; led[1] = 1;
OGA 0:4c9a3b107572 233 }else if(state == STOP){
OGA 0:4c9a3b107572 234 vl = 0;//led[0] = 0; led[1] = 0;
OGA 0:4c9a3b107572 235 }
OGA 0:4c9a3b107572 236
OGA 0:4c9a3b107572 237 vl = vl * STRAIGHT ;
OGA 0:4c9a3b107572 238 vs = vsOut() * SPIN ;
OGA 0:4c9a3b107572 239
OGA 0:4c9a3b107572 240 vl *= 0.5;
OGA 0:4c9a3b107572 241 vs *= 0.3;
OGA 0:4c9a3b107572 242
OGA 0:4c9a3b107572 243 move(vl,(int)vs);
OGA 0:4c9a3b107572 244 }
OGA 0:4c9a3b107572 245 }