ave
Dependencies: QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:3fd90568b788
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Oct 17 01:49:27 2013 +0000 @@ -0,0 +1,289 @@ +#include "mbed.h" +#include "math.h" +#include "stdlib.h" +#include "main.h" +#include "QEI.h" +#include "TextLCD.h" + +int state[2] = {0}, rope_step = 0, waiting = 0, led_check = 0; +double rev_r, rev_l, rev_p, pp = 0, rope_p = 0, power = 0, rope_s = 0; + +//超音波読み取り(割り込み) +void ping() +{ + Ultrasonic(); +} + +//回転速度読み取り(割り込み) +void speed() +{ +/* static double pulse1 = 0, pulse2 = 0; + double test; + + pulse2 = pulse1; + pulse1 = fabs((double)enc_p.getPulses()/(360*4)); + + rev_p = ave(2, pulse1 - pulse2); +*/ + rev_l = ave(0, fabs((double)enc_l.getPulses())/(360*4)); + rev_r = ave(1, fabs((double)enc_r.getPulses())/(360*4)); + rev_p = ave(2, fabs((double)enc_p.getPulses())/(360*4)); + + //printf("%03.4f %03.4f %f\n", rev_l, rev_r, pp); +/* + lcd.locate(2, 0); + lcd.printf("%1.5f", rope_p); + //lcd.printf("%03.4f", rev_l); + lcd.locate(0, 1); + lcd.printf("%03.4f", rev_p); +*/ +/* lcd.cls(); + lcd.locate(2, 0); + lcd.printf("%d", enc_p.getPulses()/2/320); + lcd.locate(0, 1); + lcd.printf("%d", enc_p.getPulses()); +*/ + + enc_l.reset(); + enc_r.reset(); + enc_p.reset(); +} + +//p制御 +double pid(double target, double sensor, double kp) +{ + double diff, p; + + diff = target - sensor; + + p = kp * diff; + + return p; +} + +void ppp() +{ +/* if(state[WHEEL]){ + pp += pid((rev_l - 0.01), rev_r, 0.05); + pwm[1] = pp; + } +*/ + if(rope_s){ + rope_p += pid(ROPE_S, rev_p, 0.001); + if(rope_p > ROPE_MAX){ + rope_p = ROPE_MAX; + } else if(rope_p < ROPE_V){ + rope_p = ROPE_V; + } + pwm[2] = rope_p; + } +} + +void rope_steps() +{ + if(rope_step == 0){ + pwm[2] = ROPE_V; + rope_s = 1; + state[ROPE] = 1; + rope_step++; + } else if(rope_step == 1){ + pwm[2] = ROPE_L; + rope_s = 0; + rope_steper.attach(&rope_steps,0.8); + rope_step++; + } else { + rope = 0; + legs = 0; + state[ROPE] = 0; + rope_step = 0; + } + /* + lcd.locate(0, 0); + lcd.printf("%d", rope_step); + */ +} + +void rope_wait() +{ + waiting = 0; +} + +void wheel_stop() +{ + wheel = 0; +} +/* +void ledwait() +{ + led_wait = 0; +} +*/ +/* +void warikomi() +{ + static int rev = 0; + + rgbled = 0x2A; + wait(0.1); + rgbled = 0x15; + + rev++; + if(rev > 1){ + myled[1] = 0; + rope_steps(); + waiter.attach(&rope_wait,1); + waiting = 1; + rev = 0; + } + + //wait(0.1); +} +*/ +int main() { + + int rev = 0; + double s_flag[4] = {0}; + + timer2.start(); + pinger.attach(&ping,0.1); + pider.attach(&ppp,0.01); + speeder.attach(&speed,0.1); + //sw.mode(PullUp); + + //sw.fall(&warikomi); + + pwm[0] = WHEEL_V; + pwm[1] = WHEEL_VL; + pwm[3] = 0.1; + pp = WHEEL_V; + rope_p = ROPE_V; + //pwm[1] = 0.2; + + start_led(); + +/* + for(;;){ + pc.printf("%05d %05d %05d %05d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]); + wait(0.1); + } +*/ + + while(1) { + //data = ave(0, ultrasonicVal[0]); + + //pc.printf("%06d %06d %06d %06d\n", ultrasonicVal[0], ultrasonicVal[1], ultrasonicVal[2], ultrasonicVal[3]); + + if(ultrasonicVal[0] < TYONPA_S){ + s_flag[0]++; + } else { + s_flag[0] = 0; + } + + if(ultrasonicVal[1] < TYONPA_S){ + s_flag[1]++; + } else { + s_flag[1] = 0; + } + + if(ultrasonicVal[2] < TYONPA_U){ + s_flag[2]++; + } else { + s_flag[2] = 0; + } + + if(ultrasonicVal[3] < TYONPA_U){ + s_flag[3]++; + } else { + s_flag[3] = 0; + } + + + if(s_flag[0] > FLAG_W){ + if(state[WHEEL] == 0){ + wheel_stoper.detach(); + } + myled[3] = 1; + state[WHEEL] = 1; + pwm[0] = WHEEL_V; + pwm[1] = WHEEL_VL; + wheel = 0x05; + rgbled = 0; + } else if(s_flag[1] > FLAG_W){ + if(state[WHEEL] == 0){ + wheel_stoper.detach(); + } + myled[2] = 1; + state[WHEEL] = 1; + pwm[0] = WHEEL_V; + pwm[1] = WHEEL_VL; + wheel = 0x0A; + rgbled = 0; + } else { + if(state[WHEEL]){ + wheel_stoper.attach(&wheel_stop,0.3); + state[WHEEL] = 0; + pwm[0] = 0; + pwm[1] = 0; + myled[3] = 0; + myled[2] = 0; + } + //wheel = 0; + rgbled = 0x15; + } + + if(s_flag[2] > FLAG_R){ + if(waiting == 0){ + if(state[ROPE] == 0){ + myled[1] = 1; + rope = 1; + pwm[2] = ROPE_V0; + rope_steper.attach(&rope_steps,1.5); + waiter.attach(&rope_wait,1); + legs = 0x02; + //wait(1); + } else { + myled[1] = 0; + rope_steps(); + waiter.attach(&rope_wait,1); + } + waiting = 1; + } + } + + if(s_flag[3] > FLAG_R){ + rgbled = 0x2A; + wait(0.1); + rgbled = 0x15; + } + + if(sw == 0){ + rgbled = 0x2A; + wait(0.1); + if(led_check == 0){ + rev++; + led_check = 1; + //led_wait = 1; + //waiter.attach(&ledwait,0.2); + if(rev > 1){ + myled[1] = 0; + rope_steps(); + waiter.attach(&rope_wait,1); + waiting = 1; + rev = 0; + } + } + } else { + led_check = 0; + } +/* + if((s_flag[0] > FLAG_W) || (s_flag[1] > FLAG_W)){ + rgbled = 0x09; + } else if((sw == 0) || (s_flag[3] > FLAG_R)){ + rgbled = 0x24; + } else { + rgbled = 0x12; + //0000000.wait(0.1); + } +*/ + } +}