Ryo Ogata / Mbed 2 deprecated jumpROBO3_5

oooga

Dependencies:   HMC6352 Servo TextLCD mbed

main.cpp@0:e0e1b495278b, 2013-09-11 (annotated)

Committer:
OGA
Date:
Wed Sep 11 05:21:06 2013 +0000
Revision:
0:e0e1b495278b
ooo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:e0e1b495278b 1 //歩くだけ
OGA 0:e0e1b495278b 2 #include "mbed.h"
OGA 0:e0e1b495278b 3 #include "ColorSensor.h"
OGA 0:e0e1b495278b 4 #include "TextLCD.h"
OGA 0:e0e1b495278b 5 #include "Servo.h"
OGA 0:e0e1b495278b 6 #include "HMC6352.h"
OGA 0:e0e1b495278b 7
OGA 0:e0e1b495278b 8 #include "main.h"
OGA 0:e0e1b495278b 9
OGA 0:e0e1b495278b 10 //センサの数
OGA 0:e0e1b495278b 11 #define COLOR_NUM 3
OGA 0:e0e1b495278b 12
OGA 0:e0e1b495278b 13 //閾値
OGA 0:e0e1b495278b 14 #define R_THR 60
OGA 0:e0e1b495278b 15 #define G_THR 60
OGA 0:e0e1b495278b 16 #define B_THR 60
OGA 0:e0e1b495278b 17
OGA 0:e0e1b495278b 18
OGA 0:e0e1b495278b 19 //TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
OGA 0:e0e1b495278b 20
OGA 0:e0e1b495278b 21 ColorSensor color0(p20, p17, p18, p19, 10);
OGA 0:e0e1b495278b 22 ColorSensor color1(p16, p13, p14, p15, 10);
OGA 0:e0e1b495278b 23 ColorSensor color2(p12, p9, p10, p11, 10);
OGA 0:e0e1b495278b 24
OGA 0:e0e1b495278b 25 Servo servoR(p23);
OGA 0:e0e1b495278b 26 Servo servoL(p24);
OGA 0:e0e1b495278b 27
OGA 0:e0e1b495278b 28 //ColorSensor color = ColorSensor();
OGA 0:e0e1b495278b 29 //Serial pc(USBTX, USBRX); // tx, rx
OGA 0:e0e1b495278b 30
OGA 0:e0e1b495278b 31 DigitalOut led[4] = {LED1,LED2,LED3,LED4};
OGA 0:e0e1b495278b 32 DigitalOut air[2] = {p21,p22};
OGA 0:e0e1b495278b 33
OGA 0:e0e1b495278b 34 Timer color_t[3];
OGA 0:e0e1b495278b 35
OGA 0:e0e1b495278b 36
OGA 0:e0e1b495278b 37 double proportional = 0;
OGA 0:e0e1b495278b 38 uint16_t com_val = 0;
OGA 0:e0e1b495278b 39
OGA 0:e0e1b495278b 40
OGA 0:e0e1b495278b 41 #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
OGA 0:e0e1b495278b 42
OGA 0:e0e1b495278b 43 #define STRAIGHT 0.6;
OGA 0:e0e1b495278b 44 #define SPIN 0.4;
OGA 0:e0e1b495278b 45
OGA 0:e0e1b495278b 46 //#define STRAIGHT 0.0;
OGA 0:e0e1b495278b 47 //#define SPIN 1.0;
OGA 0:e0e1b495278b 48
OGA 0:e0e1b495278b 49
OGA 0:e0e1b495278b 50 void move(int vl,int vs){
OGA 0:e0e1b495278b 51 double fut_R,fut_L,true_vs;
OGA 0:e0e1b495278b 52
OGA 0:e0e1b495278b 53 true_vs = abs(vs)/SPIN;
OGA 0:e0e1b495278b 54
OGA 0:e0e1b495278b 55 if(true_vs > 40){
OGA 0:e0e1b495278b 56 vl = 0;
OGA 0:e0e1b495278b 57 vs = 100*(vs/abs(vs));
OGA 0:e0e1b495278b 58 }
OGA 0:e0e1b495278b 59
OGA 0:e0e1b495278b 60 fut_R = Convert_dekaruto((vl + vs));
OGA 0:e0e1b495278b 61 fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
OGA 0:e0e1b495278b 62
OGA 0:e0e1b495278b 63 fut_R = Convert_dekaruto(-50);
OGA 0:e0e1b495278b 64 fut_L = Convert_dekaruto(-50);
OGA 0:e0e1b495278b 65
OGA 0:e0e1b495278b 66 servoR = fut_R;
OGA 0:e0e1b495278b 67 servoL = fut_L;
OGA 0:e0e1b495278b 68
OGA 0:e0e1b495278b 69 //printf("R:%lf L:%lf\n",fut_R,fut_L);
OGA 0:e0e1b495278b 70 //printf("R:%d L:%d\n",(vl + vs),-(vl - vs));
OGA 0:e0e1b495278b 71 }
OGA 0:e0e1b495278b 72
OGA 0:e0e1b495278b 73 void PidUpdate()
OGA 0:e0e1b495278b 74 {
OGA 0:e0e1b495278b 75 inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
OGA 0:e0e1b495278b 76 //printf("%lf\n",inputPID);
OGA 0:e0e1b495278b 77 }
OGA 0:e0e1b495278b 78
OGA 0:e0e1b495278b 79 double vsOut(){
OGA 0:e0e1b495278b 80 double vs;
OGA 0:e0e1b495278b 81 vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
OGA 0:e0e1b495278b 82 vs = vs * 8;
OGA 0:e0e1b495278b 83
OGA 0:e0e1b495278b 84 if(vs/abs(vs) < 0){
OGA 0:e0e1b495278b 85 //vs *= 1.3;
OGA 0:e0e1b495278b 86 }
OGA 0:e0e1b495278b 87
OGA 0:e0e1b495278b 88 if(abs(vs) > 90)vs = 90*(vs/abs(vs));
OGA 0:e0e1b495278b 89 if(abs(vs) < 25) vs = 25*(vs/abs(vs));
OGA 0:e0e1b495278b 90
OGA 0:e0e1b495278b 91 return vs;
OGA 0:e0e1b495278b 92 }
OGA 0:e0e1b495278b 93
OGA 0:e0e1b495278b 94
OGA 0:e0e1b495278b 95
OGA 0:e0e1b495278b 96 //青から赤に反応したらジャンプ
OGA 0:e0e1b495278b 97 //pidUpdataのせいでカラーセンサが動かない!!
OGA 0:e0e1b495278b 98 int main()
OGA 0:e0e1b495278b 99 {
OGA 0:e0e1b495278b 100 wait(3);
OGA 0:e0e1b495278b 101
OGA 0:e0e1b495278b 102 unsigned R, G, B;
OGA 0:e0e1b495278b 103 unsigned threshold, t[3] = {0} ;
OGA 0:e0e1b495278b 104 double color_sum;
OGA 0:e0e1b495278b 105 unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
OGA 0:e0e1b495278b 106 //pc.baud(115200);
OGA 0:e0e1b495278b 107 air[0] = 0; air[1] = 1;
OGA 0:e0e1b495278b 108
OGA 0:e0e1b495278b 109 int vl;
OGA 0:e0e1b495278b 110 double vs;
OGA 0:e0e1b495278b 111 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
OGA 0:e0e1b495278b 112 pidUpdata.attach(&PidUpdate, PID_CYCLE);
OGA 0:e0e1b495278b 113
OGA 0:e0e1b495278b 114
OGA 0:e0e1b495278b 115 while(1)
OGA 0:e0e1b495278b 116 {
OGA 0:e0e1b495278b 117
OGA 0:e0e1b495278b 118 color0.checkRGB(redp[0],greenp[0],bluep[0]);
OGA 0:e0e1b495278b 119 color1.checkRGB(redp[1],greenp[1],bluep[1]);
OGA 0:e0e1b495278b 120 color2.checkRGB(redp[2],greenp[2],bluep[2]);
OGA 0:e0e1b495278b 121
OGA 0:e0e1b495278b 122 //lcd.cls();
OGA 0:e0e1b495278b 123 //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:e0e1b495278b 124 /*
OGA 0:e0e1b495278b 125 lcd.locate(0,0);
OGA 0:e0e1b495278b 126 lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
OGA 0:e0e1b495278b 127 lcd.locate(0,1);
OGA 0:e0e1b495278b 128 lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
OGA 0:e0e1b495278b 129 */
OGA 0:e0e1b495278b 130
OGA 0:e0e1b495278b 131
OGA 0:e0e1b495278b 132
OGA 0:e0e1b495278b 133
OGA 0:e0e1b495278b 134 threshold = 0;
OGA 0:e0e1b495278b 135 for(int i=0; i<3; i++){
OGA 0:e0e1b495278b 136 if(bluep[i] >= B_THR){
OGA 0:e0e1b495278b 137 color_t[i].reset();
OGA 0:e0e1b495278b 138 color_t[i].start();
OGA 0:e0e1b495278b 139 t[i] = 0;
OGA 0:e0e1b495278b 140 }else if(redp[i] >= R_THR){
OGA 0:e0e1b495278b 141 t[i] = color_t[i].read_ms();
OGA 0:e0e1b495278b 142 }else{
OGA 0:e0e1b495278b 143 t[i] = 0;
OGA 0:e0e1b495278b 144 }
OGA 0:e0e1b495278b 145
OGA 0:e0e1b495278b 146 if((t[i] <= 500) && (t[i] != 0)){
OGA 0:e0e1b495278b 147 threshold++;
OGA 0:e0e1b495278b 148 }
OGA 0:e0e1b495278b 149 }
OGA 0:e0e1b495278b 150
OGA 0:e0e1b495278b 151
OGA 0:e0e1b495278b 152
OGA 0:e0e1b495278b 153 if(threshold >= 1){
OGA 0:e0e1b495278b 154 led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
OGA 0:e0e1b495278b 155 air[0] = 1; air[1] = 0;
OGA 0:e0e1b495278b 156 wait(0.8);
OGA 0:e0e1b495278b 157 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:e0e1b495278b 158 air[0] = 0; air[1] = 1;
OGA 0:e0e1b495278b 159 wait(0.5);
OGA 0:e0e1b495278b 160 }else{
OGA 0:e0e1b495278b 161 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:e0e1b495278b 162 }
OGA 0:e0e1b495278b 163
OGA 0:e0e1b495278b 164
OGA 0:e0e1b495278b 165
OGA 0:e0e1b495278b 166 vl = -90;
OGA 0:e0e1b495278b 167 //vl = 0;
OGA 0:e0e1b495278b 168
OGA 0:e0e1b495278b 169 vl = vl * STRAIGHT ;
OGA 0:e0e1b495278b 170 vs = vsOut() * SPIN ;
OGA 0:e0e1b495278b 171
OGA 0:e0e1b495278b 172 vl *= 0.5;
OGA 0:e0e1b495278b 173 vs *= 0.3;
OGA 0:e0e1b495278b 174
OGA 0:e0e1b495278b 175 move(vl,(int)vs);
OGA 0:e0e1b495278b 176 }
OGA 0:e0e1b495278b 177 }