Hajime Muraki
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);
+ }
+*/
+ }
+}