Hajime Muraki / Mbed 2 deprecated 1nfinity

ave

Dependencies:   QEI TextLCD mbed

main.cpp@0:3fd90568b788, 2013-10-17 (annotated)

Committer:
com3
Date:
Thu Oct 17 01:49:27 2013 +0000
Revision:
0:3fd90568b788
ave

Who changed what in which revision?

UserRevisionLine numberNew contents of line
com3 0:3fd90568b788 1 #include "mbed.h"
com3 0:3fd90568b788 2 #include "math.h"
com3 0:3fd90568b788 3 #include "stdlib.h"
com3 0:3fd90568b788 4 #include "main.h"
com3 0:3fd90568b788 5 #include "QEI.h"
com3 0:3fd90568b788 6 #include "TextLCD.h"
com3 0:3fd90568b788 7
com3 0:3fd90568b788 8 int state[2] = {0}, rope_step = 0, waiting = 0, led_check = 0;
com3 0:3fd90568b788 9 double rev_r, rev_l, rev_p, pp = 0, rope_p = 0, power = 0, rope_s = 0;
com3 0:3fd90568b788 10
com3 0:3fd90568b788 11 //超音波読み取り（割り込み）
com3 0:3fd90568b788 12 void ping()
com3 0:3fd90568b788 13 {
com3 0:3fd90568b788 14 Ultrasonic();
com3 0:3fd90568b788 15 }
com3 0:3fd90568b788 16
com3 0:3fd90568b788 17 //回転速度読み取り（割り込み）
com3 0:3fd90568b788 18 void speed()
com3 0:3fd90568b788 19 {
com3 0:3fd90568b788 20 /* static double pulse1 = 0, pulse2 = 0;
com3 0:3fd90568b788 21 double test;
com3 0:3fd90568b788 22
com3 0:3fd90568b788 23 pulse2 = pulse1;
com3 0:3fd90568b788 24 pulse1 = fabs((double)enc_p.getPulses()/(360*4));
com3 0:3fd90568b788 25
com3 0:3fd90568b788 26 rev_p = ave(2, pulse1 - pulse2);
com3 0:3fd90568b788 27 */
com3 0:3fd90568b788 28 rev_l = ave(0, fabs((double)enc_l.getPulses())/(360*4));
com3 0:3fd90568b788 29 rev_r = ave(1, fabs((double)enc_r.getPulses())/(360*4));
com3 0:3fd90568b788 30 rev_p = ave(2, fabs((double)enc_p.getPulses())/(360*4));
com3 0:3fd90568b788 31
com3 0:3fd90568b788 32 //printf("%03.4f %03.4f %f\n", rev_l, rev_r, pp);
com3 0:3fd90568b788 33 /*
com3 0:3fd90568b788 34 lcd.locate(2, 0);
com3 0:3fd90568b788 35 lcd.printf("%1.5f", rope_p);
com3 0:3fd90568b788 36 //lcd.printf("%03.4f", rev_l);
com3 0:3fd90568b788 37 lcd.locate(0, 1);
com3 0:3fd90568b788 38 lcd.printf("%03.4f", rev_p);
com3 0:3fd90568b788 39 */
com3 0:3fd90568b788 40 /* lcd.cls();
com3 0:3fd90568b788 41 lcd.locate(2, 0);
com3 0:3fd90568b788 42 lcd.printf("%d", enc_p.getPulses()/2/320);
com3 0:3fd90568b788 43 lcd.locate(0, 1);
com3 0:3fd90568b788 44 lcd.printf("%d", enc_p.getPulses());
com3 0:3fd90568b788 45 */
com3 0:3fd90568b788 46
com3 0:3fd90568b788 47 enc_l.reset();
com3 0:3fd90568b788 48 enc_r.reset();
com3 0:3fd90568b788 49 enc_p.reset();
com3 0:3fd90568b788 50 }
com3 0:3fd90568b788 51
com3 0:3fd90568b788 52 //p制御
com3 0:3fd90568b788 53 double pid(double target, double sensor, double kp)
com3 0:3fd90568b788 54 {
com3 0:3fd90568b788 55 double diff, p;
com3 0:3fd90568b788 56
com3 0:3fd90568b788 57 diff = target - sensor;
com3 0:3fd90568b788 58
com3 0:3fd90568b788 59 p = kp * diff;
com3 0:3fd90568b788 60
com3 0:3fd90568b788 61 return p;
com3 0:3fd90568b788 62 }
com3 0:3fd90568b788 63
com3 0:3fd90568b788 64 void ppp()
com3 0:3fd90568b788 65 {
com3 0:3fd90568b788 66 /* if(state[WHEEL]){
com3 0:3fd90568b788 67 pp += pid((rev_l - 0.01), rev_r, 0.05);
com3 0:3fd90568b788 68 pwm[1] = pp;
com3 0:3fd90568b788 69 }
com3 0:3fd90568b788 70 */
com3 0:3fd90568b788 71 if(rope_s){
com3 0:3fd90568b788 72 rope_p += pid(ROPE_S, rev_p, 0.001);
com3 0:3fd90568b788 73 if(rope_p > ROPE_MAX){
com3 0:3fd90568b788 74 rope_p = ROPE_MAX;
com3 0:3fd90568b788 75 } else if(rope_p < ROPE_V){
com3 0:3fd90568b788 76 rope_p = ROPE_V;
com3 0:3fd90568b788 77 }
com3 0:3fd90568b788 78 pwm[2] = rope_p;
com3 0:3fd90568b788 79 }
com3 0:3fd90568b788 80 }
com3 0:3fd90568b788 81
com3 0:3fd90568b788 82 void rope_steps()
com3 0:3fd90568b788 83 {
com3 0:3fd90568b788 84 if(rope_step == 0){
com3 0:3fd90568b788 85 pwm[2] = ROPE_V;
com3 0:3fd90568b788 86 rope_s = 1;
com3 0:3fd90568b788 87 state[ROPE] = 1;
com3 0:3fd90568b788 88 rope_step++;
com3 0:3fd90568b788 89 } else if(rope_step == 1){
com3 0:3fd90568b788 90 pwm[2] = ROPE_L;
com3 0:3fd90568b788 91 rope_s = 0;
com3 0:3fd90568b788 92 rope_steper.attach(&rope_steps,0.8);
com3 0:3fd90568b788 93 rope_step++;
com3 0:3fd90568b788 94 } else {
com3 0:3fd90568b788 95 rope = 0;
com3 0:3fd90568b788 96 legs = 0;
com3 0:3fd90568b788 97 state[ROPE] = 0;
com3 0:3fd90568b788 98 rope_step = 0;
com3 0:3fd90568b788 99 }
com3 0:3fd90568b788 100 /*
com3 0:3fd90568b788 101 lcd.locate(0, 0);
com3 0:3fd90568b788 102 lcd.printf("%d", rope_step);
com3 0:3fd90568b788 103 */
com3 0:3fd90568b788 104 }
com3 0:3fd90568b788 105
com3 0:3fd90568b788 106 void rope_wait()
com3 0:3fd90568b788 107 {
com3 0:3fd90568b788 108 waiting = 0;
com3 0:3fd90568b788 109 }
com3 0:3fd90568b788 110
com3 0:3fd90568b788 111 void wheel_stop()
com3 0:3fd90568b788 112 {
com3 0:3fd90568b788 113 wheel = 0;
com3 0:3fd90568b788 114 }
com3 0:3fd90568b788 115 /*
com3 0:3fd90568b788 116 void ledwait()
com3 0:3fd90568b788 117 {
com3 0:3fd90568b788 118 led_wait = 0;
com3 0:3fd90568b788 119 }
com3 0:3fd90568b788 120 */
com3 0:3fd90568b788 121 /*
com3 0:3fd90568b788 122 void warikomi()
com3 0:3fd90568b788 123 {
com3 0:3fd90568b788 124 static int rev = 0;
com3 0:3fd90568b788 125
com3 0:3fd90568b788 126 rgbled = 0x2A;
com3 0:3fd90568b788 127 wait(0.1);
com3 0:3fd90568b788 128 rgbled = 0x15;
com3 0:3fd90568b788 129
com3 0:3fd90568b788 130 rev++;
com3 0:3fd90568b788 131 if(rev > 1){
com3 0:3fd90568b788 132 myled[1] = 0;
com3 0:3fd90568b788 133 rope_steps();
com3 0:3fd90568b788 134 waiter.attach(&rope_wait,1);
com3 0:3fd90568b788 135 waiting = 1;
com3 0:3fd90568b788 136 rev = 0;
com3 0:3fd90568b788 137 }
com3 0:3fd90568b788 138
com3 0:3fd90568b788 139 //wait(0.1);
com3 0:3fd90568b788 140 }
com3 0:3fd90568b788 141 */
com3 0:3fd90568b788 142 int main() {
com3 0:3fd90568b788 143
com3 0:3fd90568b788 144 int rev = 0;
com3 0:3fd90568b788 145 double s_flag[4] = {0};
com3 0:3fd90568b788 146
com3 0:3fd90568b788 147 timer2.start();
com3 0:3fd90568b788 148 pinger.attach(&ping,0.1);
com3 0:3fd90568b788 149 pider.attach(&ppp,0.01);
com3 0:3fd90568b788 150 speeder.attach(&speed,0.1);
com3 0:3fd90568b788 151 //sw.mode(PullUp);
com3 0:3fd90568b788 152
com3 0:3fd90568b788 153 //sw.fall(&warikomi);
com3 0:3fd90568b788 154
com3 0:3fd90568b788 155 pwm[0] = WHEEL_V;
com3 0:3fd90568b788 156 pwm[1] = WHEEL_VL;
com3 0:3fd90568b788 157 pwm[3] = 0.1;
com3 0:3fd90568b788 158 pp = WHEEL_V;
com3 0:3fd90568b788 159 rope_p = ROPE_V;
com3 0:3fd90568b788 160 //pwm[1] = 0.2;
com3 0:3fd90568b788 161
com3 0:3fd90568b788 162 start_led();
com3 0:3fd90568b788 163
com3 0:3fd90568b788 164 /*
com3 0:3fd90568b788 165 for(;;){
com3 0:3fd90568b788 166 pc.printf("%05d %05d %05d %05d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]);
com3 0:3fd90568b788 167 wait(0.1);
com3 0:3fd90568b788 168 }
com3 0:3fd90568b788 169 */
com3 0:3fd90568b788 170
com3 0:3fd90568b788 171 while(1) {
com3 0:3fd90568b788 172 //data = ave(0, ultrasonicVal[0]);
com3 0:3fd90568b788 173
com3 0:3fd90568b788 174 //pc.printf("%06d %06d %06d %06d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]);
com3 0:3fd90568b788 175
com3 0:3fd90568b788 176 if(ultrasonicVal[0] < TYONPA_S){
com3 0:3fd90568b788 177 s_flag[0]++;
com3 0:3fd90568b788 178 } else {
com3 0:3fd90568b788 179 s_flag[0] = 0;
com3 0:3fd90568b788 180 }
com3 0:3fd90568b788 181
com3 0:3fd90568b788 182 if(ultrasonicVal[1] < TYONPA_S){
com3 0:3fd90568b788 183 s_flag[1]++;
com3 0:3fd90568b788 184 } else {
com3 0:3fd90568b788 185 s_flag[1] = 0;
com3 0:3fd90568b788 186 }
com3 0:3fd90568b788 187
com3 0:3fd90568b788 188 if(ultrasonicVal[2] < TYONPA_U){
com3 0:3fd90568b788 189 s_flag[2]++;
com3 0:3fd90568b788 190 } else {
com3 0:3fd90568b788 191 s_flag[2] = 0;
com3 0:3fd90568b788 192 }
com3 0:3fd90568b788 193
com3 0:3fd90568b788 194 if(ultrasonicVal[3] < TYONPA_U){
com3 0:3fd90568b788 195 s_flag[3]++;
com3 0:3fd90568b788 196 } else {
com3 0:3fd90568b788 197 s_flag[3] = 0;
com3 0:3fd90568b788 198 }
com3 0:3fd90568b788 199
com3 0:3fd90568b788 200
com3 0:3fd90568b788 201 if(s_flag[0] > FLAG_W){
com3 0:3fd90568b788 202 if(state[WHEEL] == 0){
com3 0:3fd90568b788 203 wheel_stoper.detach();
com3 0:3fd90568b788 204 }
com3 0:3fd90568b788 205 myled[3] = 1;
com3 0:3fd90568b788 206 state[WHEEL] = 1;
com3 0:3fd90568b788 207 pwm[0] = WHEEL_V;
com3 0:3fd90568b788 208 pwm[1] = WHEEL_VL;
com3 0:3fd90568b788 209 wheel = 0x05;
com3 0:3fd90568b788 210 rgbled = 0;
com3 0:3fd90568b788 211 } else if(s_flag[1] > FLAG_W){
com3 0:3fd90568b788 212 if(state[WHEEL] == 0){
com3 0:3fd90568b788 213 wheel_stoper.detach();
com3 0:3fd90568b788 214 }
com3 0:3fd90568b788 215 myled[2] = 1;
com3 0:3fd90568b788 216 state[WHEEL] = 1;
com3 0:3fd90568b788 217 pwm[0] = WHEEL_V;
com3 0:3fd90568b788 218 pwm[1] = WHEEL_VL;
com3 0:3fd90568b788 219 wheel = 0x0A;
com3 0:3fd90568b788 220 rgbled = 0;
com3 0:3fd90568b788 221 } else {
com3 0:3fd90568b788 222 if(state[WHEEL]){
com3 0:3fd90568b788 223 wheel_stoper.attach(&wheel_stop,0.3);
com3 0:3fd90568b788 224 state[WHEEL] = 0;
com3 0:3fd90568b788 225 pwm[0] = 0;
com3 0:3fd90568b788 226 pwm[1] = 0;
com3 0:3fd90568b788 227 myled[3] = 0;
com3 0:3fd90568b788 228 myled[2] = 0;
com3 0:3fd90568b788 229 }
com3 0:3fd90568b788 230 //wheel = 0;
com3 0:3fd90568b788 231 rgbled = 0x15;
com3 0:3fd90568b788 232 }
com3 0:3fd90568b788 233
com3 0:3fd90568b788 234 if(s_flag[2] > FLAG_R){
com3 0:3fd90568b788 235 if(waiting == 0){
com3 0:3fd90568b788 236 if(state[ROPE] == 0){
com3 0:3fd90568b788 237 myled[1] = 1;
com3 0:3fd90568b788 238 rope = 1;
com3 0:3fd90568b788 239 pwm[2] = ROPE_V0;
com3 0:3fd90568b788 240 rope_steper.attach(&rope_steps,1.5);
com3 0:3fd90568b788 241 waiter.attach(&rope_wait,1);
com3 0:3fd90568b788 242 legs = 0x02;
com3 0:3fd90568b788 243 //wait(1);
com3 0:3fd90568b788 244 } else {
com3 0:3fd90568b788 245 myled[1] = 0;
com3 0:3fd90568b788 246 rope_steps();
com3 0:3fd90568b788 247 waiter.attach(&rope_wait,1);
com3 0:3fd90568b788 248 }
com3 0:3fd90568b788 249 waiting = 1;
com3 0:3fd90568b788 250 }
com3 0:3fd90568b788 251 }
com3 0:3fd90568b788 252
com3 0:3fd90568b788 253 if(s_flag[3] > FLAG_R){
com3 0:3fd90568b788 254 rgbled = 0x2A;
com3 0:3fd90568b788 255 wait(0.1);
com3 0:3fd90568b788 256 rgbled = 0x15;
com3 0:3fd90568b788 257 }
com3 0:3fd90568b788 258
com3 0:3fd90568b788 259 if(sw == 0){
com3 0:3fd90568b788 260 rgbled = 0x2A;
com3 0:3fd90568b788 261 wait(0.1);
com3 0:3fd90568b788 262 if(led_check == 0){
com3 0:3fd90568b788 263 rev++;
com3 0:3fd90568b788 264 led_check = 1;
com3 0:3fd90568b788 265 //led_wait = 1;
com3 0:3fd90568b788 266 //waiter.attach(&ledwait,0.2);
com3 0:3fd90568b788 267 if(rev > 1){
com3 0:3fd90568b788 268 myled[1] = 0;
com3 0:3fd90568b788 269 rope_steps();
com3 0:3fd90568b788 270 waiter.attach(&rope_wait,1);
com3 0:3fd90568b788 271 waiting = 1;
com3 0:3fd90568b788 272 rev = 0;
com3 0:3fd90568b788 273 }
com3 0:3fd90568b788 274 }
com3 0:3fd90568b788 275 } else {
com3 0:3fd90568b788 276 led_check = 0;
com3 0:3fd90568b788 277 }
com3 0:3fd90568b788 278 /*
com3 0:3fd90568b788 279 if((s_flag[0] > FLAG_W) || (s_flag[1] > FLAG_W)){
com3 0:3fd90568b788 280 rgbled = 0x09;
com3 0:3fd90568b788 281 } else if((sw == 0) || (s_flag[3] > FLAG_R)){
com3 0:3fd90568b788 282 rgbled = 0x24;
com3 0:3fd90568b788 283 } else {
com3 0:3fd90568b788 284 rgbled = 0x12;
com3 0:3fd90568b788 285 //0000000.wait(0.1);
com3 0:3fd90568b788 286 }
com3 0:3fd90568b788 287 */
com3 0:3fd90568b788 288 }
com3 0:3fd90568b788 289 }