Hajime Muraki
ave
Dependencies: QEI TextLCD mbed
Revision 0:3fd90568b788, committed 2013-10-17
- Comitter:
- com3
- Date:
- Thu Oct 17 01:49:27 2013 +0000
- Commit message:
- ave
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu Oct 17 01:49:27 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Thu Oct 17 01:49:27 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/average/ave.cpp Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,19 @@
+#include "mbed.h"
+
+#define KAZU 3
+
+double ave(int num, double value)
+{
+ static double sum[KAZU] = {0};
+ static double data[KAZU][5] = {{0}};
+
+ sum[num] -= data[num][4];
+ sum[num] += value;
+ data[num][4] = data[num][3];
+ data[num][3] = data[num][2];
+ data[num][2] = data[num][1];
+ data[num][1] = data[num][0];
+ data[num][0] = value;
+
+ return sum[num]/5;
+}
\ No newline at end of file
--- /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);
+ }
+*/
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,53 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "TextLCD.h"
+
+#define WHEEL 0
+#define WHEEL_V 0.5 //left
+#define WHEEL_VL 0.2 //right
+#define FLAG_W 3
+
+#define ROPE 1
+#define ROPE_V0 0.18
+#define ROPE_MAX 0.08
+#define ROPE_V 0.05
+#define ROPE_S 0.05
+#define ROPE_L 0.001
+#define FLAG_R 5
+
+#define TYONPA_S 700
+#define TYONPA_U 600
+#define FLAG 5
+
+Timer timer2;
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
+extern void start_led(void);
+extern double ave(int num, double value);
+
+
+BusOut wheel(p9, p10, p11, p12);
+BusOut rope(p13, p14);
+BusOut legs(p15, p16);
+BusOut rgbled(p17, p18, p19);
+PwmOut pwm[4] = {p21, p22, p23, p24};
+DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
+DigitalIn sw(p20);
+//InterruptIn sw(p24);
+QEI enc_l(p25, p26, NC, 360, QEI::X4_ENCODING);
+QEI enc_r(p27, p28, NC, 360, QEI::X4_ENCODING);
+QEI enc_p(p29, p30, NC, 360, QEI::X4_ENCODING);
+//TextLCD lcd(p30, p29, p28, p27, p26, p25);
+
+Ticker speeder;
+Ticker pider;
+Ticker pinger;
+
+Timeout rope_steper;
+Timeout rbleder;
+Timeout waiter;
+Timeout wheel_stoper;
+
+Serial pc(USBTX, USBRX);
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Oct 17 01:49:27 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9c8f0e3462fb \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.cpp Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "ping.h"
+
+//DigitalOut myled = LED1;
+
+extern Timer timer2;
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+ for(int i = 0 ; i < ALL_ULTRASONIC; i++){
+
+ uint8_t flag = 0;
+
+ DigitalOut PingPinOut(ultrasonic_pin[i]);
+ PingPinOut = 1;
+ wait_us(10);
+ PingPinOut = 0;
+ DigitalIn PingPin(ultrasonic_pin[i]);
+ timer2.reset();
+ while(PingPin == 0){
+ if(timer2.read_us() > 1500){ //1.5ms以上応答なし
+ ultrasonicValue[i] = PING_ERR;
+ flag = 1;
+ break;
+ }
+ }
+
+ timer2.reset();
+ while(PingPin == 1){
+ if((timer2.read_us() > 18500) || (flag == 1)){ //18.5ms以上のパルス
+ ultrasonicValue[i] = PING_ERR;
+ flag = 1;
+ break;
+ }
+ }
+
+ if(flag == 0){
+ ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm MAX:3145
+ ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+ }else{
+ ultrasonicVal[i] = PING_ERR;
+ }
+
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.h Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,24 @@
+#define ALL_ULTRASONIC 4
+#define PING_ERR 0xFFFF
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+ p5,
+ p6,
+ p7,
+ p8,
+};
+
+
+
+/*
+#define ALL_ULTRASONIC 4
+#define PING_ERR 0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+ p17,
+ p18,
+ p19,
+ p20,
+};
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/start_led.cpp Thu Oct 17 01:49:27 2013 +0000
@@ -0,0 +1,16 @@
+#include "mbed.h"
+
+BusOut myleds(LED1, LED2, LED3, LED4);
+
+void start_led()
+{
+ myleds = 0x08;
+ wait(1);
+ myleds = 0x04;
+ wait(1);
+ myleds = 0x02;
+ wait(1);
+ myleds = 0x01;
+ wait(1);
+ myleds = 0;
+}
\ No newline at end of file