Ryo Ogata / Mbed 2 deprecated jumpROBO4_3

jump!

Dependencies:   ColorSensor1 HMC6352 Servo TextLCD mbed

main.cpp@0:feefc7aaa114, 2013-09-23 (annotated)

Committer:
OGA
Date:
Mon Sep 23 06:35:28 2013 +0000
Revision:
0:feefc7aaa114
var.4.3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:feefc7aaa114 1 #include "mbed.h"
OGA 0:feefc7aaa114 2 #include "ColorSensor.h"
OGA 0:feefc7aaa114 3 #include "TextLCD.h"
OGA 0:feefc7aaa114 4 #include "Servo.h"
OGA 0:feefc7aaa114 5 #include "HMC6352.h"
OGA 0:feefc7aaa114 6
OGA 0:feefc7aaa114 7 #include "main.h"
OGA 0:feefc7aaa114 8
OGA 0:feefc7aaa114 9
OGA 0:feefc7aaa114 10 void tic_sensor()
OGA 0:feefc7aaa114 11 {
OGA 0:feefc7aaa114 12 Ultrasonic();
OGA 0:feefc7aaa114 13
OGA 0:feefc7aaa114 14 colorUpdate();
OGA 0:feefc7aaa114 15
OGA 0:feefc7aaa114 16 /*lcd.cls();
OGA 0:feefc7aaa114 17 lcd.locate(0,0);
OGA 0:feefc7aaa114 18 lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:feefc7aaa114 19 */
OGA 0:feefc7aaa114 20 }
OGA 0:feefc7aaa114 21
OGA 0:feefc7aaa114 22 ////////////////////////////////////////移動関数//////////////////////////////////////////////
OGA 0:feefc7aaa114 23 /////////////////////////////////////////////////////////////////////////////////////////////
OGA 0:feefc7aaa114 24 void move(int vl,int vs){
OGA 0:feefc7aaa114 25 double fut_R,fut_L,true_vs;
OGA 0:feefc7aaa114 26
OGA 0:feefc7aaa114 27 true_vs = abs(vs)/SPIN;
OGA 0:feefc7aaa114 28
OGA 0:feefc7aaa114 29 if(true_vs > 40){
OGA 0:feefc7aaa114 30 vl = 0;
OGA 0:feefc7aaa114 31 vs = 100*(vs/abs(vs));
OGA 0:feefc7aaa114 32 }
OGA 0:feefc7aaa114 33
OGA 0:feefc7aaa114 34 fut_R = Convert_dekaruto((vl + vs));
OGA 0:feefc7aaa114 35 fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
OGA 0:feefc7aaa114 36
OGA 0:feefc7aaa114 37 fut_R = Convert_dekaruto(-50);
OGA 0:feefc7aaa114 38 fut_L = Convert_dekaruto(-50);
OGA 0:feefc7aaa114 39
OGA 0:feefc7aaa114 40 servoR = fut_R;
OGA 0:feefc7aaa114 41 servoL = fut_L;
OGA 0:feefc7aaa114 42
OGA 0:feefc7aaa114 43 //printf("R:%lf L:%lf\n",fut_R,fut_L);
OGA 0:feefc7aaa114 44 //printf("R:%d L:%d\n",(vl + vs),-(vl - vs));
OGA 0:feefc7aaa114 45 }
OGA 0:feefc7aaa114 46
OGA 0:feefc7aaa114 47 /*void PidUpdate()
OGA 0:feefc7aaa114 48 {
OGA 0:feefc7aaa114 49 inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
OGA 0:feefc7aaa114 50 //printf("%lf\n",inputPID);
OGA 0:feefc7aaa114 51 }*/
OGA 0:feefc7aaa114 52
OGA 0:feefc7aaa114 53 double vsOut(){
OGA 0:feefc7aaa114 54 double vs;
OGA 0:feefc7aaa114 55 vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
OGA 0:feefc7aaa114 56 vs = vs * 8;
OGA 0:feefc7aaa114 57
OGA 0:feefc7aaa114 58 if(vs/abs(vs) < 0){
OGA 0:feefc7aaa114 59 //vs *= 1.3;
OGA 0:feefc7aaa114 60 }
OGA 0:feefc7aaa114 61
OGA 0:feefc7aaa114 62 if(abs(vs) > 90)vs = 90*(vs/abs(vs));
OGA 0:feefc7aaa114 63 if(abs(vs) < 25) vs = 25*(vs/abs(vs));
OGA 0:feefc7aaa114 64
OGA 0:feefc7aaa114 65 return vs;
OGA 0:feefc7aaa114 66 }
OGA 0:feefc7aaa114 67
OGA 0:feefc7aaa114 68 ////////////////////////////////////////超音波センサの////////////////////////////////////////
OGA 0:feefc7aaa114 69 ////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
OGA 0:feefc7aaa114 70 int ping_button(int ping,int button){
OGA 0:feefc7aaa114 71 static int continue_flag = 0;
OGA 0:feefc7aaa114 72 static int change_flag = 0;
OGA 0:feefc7aaa114 73
OGA 0:feefc7aaa114 74 if(continue_flag == 0){
OGA 0:feefc7aaa114 75 if(ping <= PINR_THR){
OGA 0:feefc7aaa114 76 ping_t.start();
OGA 0:feefc7aaa114 77 continue_flag = 1;
OGA 0:feefc7aaa114 78 }
OGA 0:feefc7aaa114 79 }
OGA 0:feefc7aaa114 80
OGA 0:feefc7aaa114 81 if(continue_flag == 1){
OGA 0:feefc7aaa114 82 //agatterutoki
OGA 0:feefc7aaa114 83 if(ping <= PINR_THR){
OGA 0:feefc7aaa114 84 if(change_flag == 0){
OGA 0:feefc7aaa114 85 if(ping_t.read_ms() >= 300){
OGA 0:feefc7aaa114 86 button = !button;
OGA 0:feefc7aaa114 87 change_flag = 1;
OGA 0:feefc7aaa114 88 }
OGA 0:feefc7aaa114 89 }
OGA 0:feefc7aaa114 90 }
OGA 0:feefc7aaa114 91 //tatisagari
OGA 0:feefc7aaa114 92 if(ping >= (PINR_THR+200)){
OGA 0:feefc7aaa114 93 ping_t.stop();
OGA 0:feefc7aaa114 94 ping_t.reset();
OGA 0:feefc7aaa114 95 continue_flag = 0;
OGA 0:feefc7aaa114 96 change_flag = 0;
OGA 0:feefc7aaa114 97 }
OGA 0:feefc7aaa114 98 }
OGA 0:feefc7aaa114 99 return button;
OGA 0:feefc7aaa114 100 }
OGA 0:feefc7aaa114 101
OGA 0:feefc7aaa114 102 ////////////////////////////////////////カラーセンサの////////////////////////////////////////
OGA 0:feefc7aaa114 103 ////////////////////////////////////////補正プログラム////////////////////////////////////////
OGA 0:feefc7aaa114 104 void rivisedate()
OGA 0:feefc7aaa114 105 {
OGA 0:feefc7aaa114 106 unsigned long red = 0,green = 0,blue =0;
OGA 0:feefc7aaa114 107 static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
OGA 0:feefc7aaa114 108
OGA 0:feefc7aaa114 109 //最初の20回だけ平均を取る
OGA 0:feefc7aaa114 110 for (int i=0;i<=20;i++){
OGA 0:feefc7aaa114 111 color0.getRGB(R[0],G[0],B[0]);
OGA 0:feefc7aaa114 112 red += R[0] ;
OGA 0:feefc7aaa114 113 green += G[0] ;
OGA 0:feefc7aaa114 114 blue += B[0] ;
OGA 0:feefc7aaa114 115 //pc.printf(" %d %d\n",ptm(sum),sum);
OGA 0:feefc7aaa114 116 }
OGA 0:feefc7aaa114 117
OGA 0:feefc7aaa114 118 rir = (double)green/ red ;
OGA 0:feefc7aaa114 119 rib = (double)green/ blue ;
OGA 0:feefc7aaa114 120 }
OGA 0:feefc7aaa114 121
OGA 0:feefc7aaa114 122 void colorUpdate()
OGA 0:feefc7aaa114 123 {
OGA 0:feefc7aaa114 124 double colorSum[COLOR_NUM];
OGA 0:feefc7aaa114 125 unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
OGA 0:feefc7aaa114 126
OGA 0:feefc7aaa114 127 color0.getRGB(R[0],G[0],B[0]);
OGA 0:feefc7aaa114 128 color1.getRGB(R[1],G[1],B[1]);
OGA 0:feefc7aaa114 129 color2.getRGB(R[2],G[2],B[2]);
OGA 0:feefc7aaa114 130 /*color3.getRGB(R[3],G[3],B[3]);
OGA 0:feefc7aaa114 131 color4.getRGB(R[4],G[4],B[4]);
OGA 0:feefc7aaa114 132 color5.getRGB(R[5],G[5],B[5]);*/
OGA 0:feefc7aaa114 133
OGA 0:feefc7aaa114 134 for (int i=0; i<COLOR_NUM; i++){
OGA 0:feefc7aaa114 135 colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
OGA 0:feefc7aaa114 136 redp[i] = R[i]* rir * 100 / colorSum[i];
OGA 0:feefc7aaa114 137 greenp[i] = G[i] * 100 / colorSum[i];
OGA 0:feefc7aaa114 138 bluep[i] = B[i]* rib * 100 / colorSum[i];
OGA 0:feefc7aaa114 139 }
OGA 0:feefc7aaa114 140 }
OGA 0:feefc7aaa114 141
OGA 0:feefc7aaa114 142 ////////////////////////////////////////ジャンププログラム////////////////////////////////////
OGA 0:feefc7aaa114 143 ///////////////////////////////////////////////////////////////////////////////////////////
OGA 0:feefc7aaa114 144 uint8_t jumpcondition()
OGA 0:feefc7aaa114 145 {
OGA 0:feefc7aaa114 146 uint8_t threshold = 0, t[3] = {0};
OGA 0:feefc7aaa114 147
OGA 0:feefc7aaa114 148 //青から赤に0.5秒以内に反応したらジャンプ
OGA 0:feefc7aaa114 149 for(int i=0; i<COLOR_NUM; i++){
OGA 0:feefc7aaa114 150 if(bluep[i] >= B_THR){
OGA 0:feefc7aaa114 151 color_t[i].reset();
OGA 0:feefc7aaa114 152 color_t[i].start();
OGA 0:feefc7aaa114 153 t[i] = 0;
OGA 0:feefc7aaa114 154 }else if(redp[i] >= R_THR){
OGA 0:feefc7aaa114 155 t[i] = color_t[i].read_ms();
OGA 0:feefc7aaa114 156 }else{
OGA 0:feefc7aaa114 157 t[i] = 0;
OGA 0:feefc7aaa114 158 }
OGA 0:feefc7aaa114 159
OGA 0:feefc7aaa114 160 if((t[i] <= 500) && (t[i] != 0)){
OGA 0:feefc7aaa114 161 threshold++;
OGA 0:feefc7aaa114 162 }
OGA 0:feefc7aaa114 163 }
OGA 0:feefc7aaa114 164
OGA 0:feefc7aaa114 165 return threshold;
OGA 0:feefc7aaa114 166 }
OGA 0:feefc7aaa114 167
OGA 0:feefc7aaa114 168 void jumping(uint8_t threshold)
OGA 0:feefc7aaa114 169 {
OGA 0:feefc7aaa114 170 //超音波でジャンプのタイミング合わせる
OGA 0:feefc7aaa114 171 if(threshold >= 1){
OGA 0:feefc7aaa114 172 jump_t.reset();
OGA 0:feefc7aaa114 173 jump_t.start();
OGA 0:feefc7aaa114 174 while(ultrasonicVal[0] < 1700){
OGA 0:feefc7aaa114 175 led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
OGA 0:feefc7aaa114 176 air[0] = 1; air[1] = 0;
OGA 0:feefc7aaa114 177
OGA 0:feefc7aaa114 178 if(jump_t.read_ms() > 1000)break;
OGA 0:feefc7aaa114 179 }
OGA 0:feefc7aaa114 180 led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
OGA 0:feefc7aaa114 181 air[0] = 0; air[1] = 1;
OGA 0:feefc7aaa114 182 wait(0.5);
OGA 0:feefc7aaa114 183 }else{
OGA 0:feefc7aaa114 184 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:feefc7aaa114 185 }
OGA 0:feefc7aaa114 186 }
OGA 0:feefc7aaa114 187
OGA 0:feefc7aaa114 188
OGA 0:feefc7aaa114 189
OGA 0:feefc7aaa114 190
OGA 0:feefc7aaa114 191
OGA 0:feefc7aaa114 192
OGA 0:feefc7aaa114 193
OGA 0:feefc7aaa114 194
OGA 0:feefc7aaa114 195
OGA 0:feefc7aaa114 196
OGA 0:feefc7aaa114 197
OGA 0:feefc7aaa114 198 int main()
OGA 0:feefc7aaa114 199 {
OGA 0:feefc7aaa114 200 rivisedate();
OGA 0:feefc7aaa114 201
OGA 0:feefc7aaa114 202 timer2.start();
OGA 0:feefc7aaa114 203 ping_t.start();
OGA 0:feefc7aaa114 204
OGA 0:feefc7aaa114 205
OGA 0:feefc7aaa114 206 //unsigned R, G, B;
OGA 0:feefc7aaa114 207 int vl;
OGA 0:feefc7aaa114 208 double vs;
OGA 0:feefc7aaa114 209 uint8_t button, state=0;
OGA 0:feefc7aaa114 210
OGA 0:feefc7aaa114 211 //pc.baud(115200);
OGA 0:feefc7aaa114 212 air[0] = 0; air[1] = 1;
OGA 0:feefc7aaa114 213
OGA 0:feefc7aaa114 214
OGA 0:feefc7aaa114 215 //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
OGA 0:feefc7aaa114 216 //pidUpdata.attach(&PidUpdate, PID_CYCLE);
OGA 0:feefc7aaa114 217 interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
OGA 0:feefc7aaa114 218
OGA 0:feefc7aaa114 219 while(1)
OGA 0:feefc7aaa114 220 {
OGA 0:feefc7aaa114 221
OGA 0:feefc7aaa114 222
OGA 0:feefc7aaa114 223
OGA 0:feefc7aaa114 224 pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:feefc7aaa114 225 /*
OGA 0:feefc7aaa114 226 lcd.cls();
OGA 0:feefc7aaa114 227 lcd.locate(0,0);
OGA 0:feefc7aaa114 228 lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
OGA 0:feefc7aaa114 229 lcd.locate(0,1);
OGA 0:feefc7aaa114 230 lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
OGA 0:feefc7aaa114 231 */
OGA 0:feefc7aaa114 232 //pc.printf("%d\n", ultrasonicVal[0]);
OGA 0:feefc7aaa114 233
OGA 0:feefc7aaa114 234
OGA 0:feefc7aaa114 235 jumping(jumpcondition());
OGA 0:feefc7aaa114 236
OGA 0:feefc7aaa114 237
OGA 0:feefc7aaa114 238
OGA 0:feefc7aaa114 239 button = ping_button(ultrasonicVal[1],button);
OGA 0:feefc7aaa114 240
OGA 0:feefc7aaa114 241 if(button){
OGA 0:feefc7aaa114 242 state = GO;
OGA 0:feefc7aaa114 243 }else{
OGA 0:feefc7aaa114 244 state = STOP;
OGA 0:feefc7aaa114 245 }
OGA 0:feefc7aaa114 246
OGA 0:feefc7aaa114 247
OGA 0:feefc7aaa114 248
OGA 0:feefc7aaa114 249
OGA 0:feefc7aaa114 250
OGA 0:feefc7aaa114 251 if(state == GO){
OGA 0:feefc7aaa114 252 vl = -90;//led[0] = 1; led[1] = 1;
OGA 0:feefc7aaa114 253 }else if(state == STOP){
OGA 0:feefc7aaa114 254 vl = 0;//led[0] = 0; led[1] = 0;
OGA 0:feefc7aaa114 255 }
OGA 0:feefc7aaa114 256
OGA 0:feefc7aaa114 257 vl = vl * STRAIGHT ;
OGA 0:feefc7aaa114 258 vs = vsOut() * SPIN ;
OGA 0:feefc7aaa114 259
OGA 0:feefc7aaa114 260 vl *= 0.5;
OGA 0:feefc7aaa114 261 vs *= 0.3;
OGA 0:feefc7aaa114 262
OGA 0:feefc7aaa114 263 move(vl,(int)vs);
OGA 0:feefc7aaa114 264 }
OGA 0:feefc7aaa114 265 }