Revision 0:ae5d19a716e1, committed 2014-03-05

Comitter:

Nishii

Date:

Wed Mar 05 05:17:45 2014 +0000

Commit message:

for Robot Grand Prix

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/control.cpp	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,269 @@
+/* control.cpp */
+
+#include "global.h"
+
+/**
+ * Functions
+ */
+void reset_A() {
+    
+    pc.printf( "A " );
+    if(MS_A.read()) {
+        setspeed(dutyr_A, _A_);
+        while(MS_A.read()) {
+            wait(0.05);
+        }
+    }
+    setspeed(STOP, _A_);
+    pc.printf( "OK \r\n" );
+}
+
+void reset_U() {
+    
+    pc.printf( "U " );
+    if(MS_U.read()) {
+        setspeed(dutyu_U, _U_);
+        while(MS_U.read()) {
+            wait(0.05);
+        }
+    }
+    setspeed(STOP, _U_);
+    hight.reset();
+    pc.printf( "OK \r\n" );
+}
+
+void reset_M() {
+    
+    pc.printf( "M " );
+    if(MS_M.read()) {
+        setspeed(dutyr_M, _M_);
+        while(MS_M.read()) {
+            wait(0.05);
+        }
+    }
+    setspeed(STOP, _M_);
+    pc.printf( "OK \r\n" );
+}
+
+void reset_S() {
+    
+    pc.printf( "S " );
+    if(MS_S.read()) {
+        setspeed(dutyr_S, _S_);
+        while(MS_S.read()) {
+            wait(0.01);
+        }
+    }
+    setspeed(STOP, _S_);
+    pc.printf( "OK \r\n" );
+}
+
+void reset() {
+    
+    reset_A();
+    reset_U();
+    reset_M();
+    reset_S();
+}
+
+void cook_stop() {
+    if(pc.getc()=='s') {
+        pc.printf( "wait a momnet \r\n" );
+        ALLSTOP();
+        pc.printf( "input \r\n" );
+        pc.getc();
+        STOP_STATUS = false;
+    }
+}
+
+void AUTO() {
+    
+    char c;
+    pc.printf( "A" );
+    wait(0.5);
+    pc.printf( "auto mode \r\n" );
+    while(1) {
+        c = pc.getc();
+        if(c=='b')  break;
+        switch (c) {
+            case 'R':
+                pc.printf( "reset \r\n" );
+                reset();
+                break;
+            case 'r':
+                c = pc.getc();
+                switch (c) {
+                    case 'A':
+                        reset_A();
+                        break;
+                    case 'U':
+                        reset_U();
+                        break;
+                    case 'M':
+                        reset_M();
+                        break;
+                    case 'S':
+                        reset_S();
+                        break;
+                }
+                break;
+            case 'f':
+                pc.printf( "start \r\n" );
+                cook();
+                //pc.printf( "finish \r\n" );
+                break;
+        }
+    }
+}
+
+void MANUAL() {
+    
+    char c;
+    double step;
+    pc.printf( "M" );
+    wait(0.5);
+    pc.printf( "manual mode \r\n" );
+    while(1) {
+        c = pc.getc();
+        if(c=='b')  {
+            ALLSTOP();
+            pc.printf( "all stop \r\n" );
+            break;
+        }
+        else if(c=='s') {
+            ALLSTOP();
+            led_all(OFF);
+            pc.printf( "all stop \r\n" );
+            continue;
+        }
+        else if(c=='w') {
+            step = 0.05;
+            c = pc.getc();
+        }
+        else if(c=='n') {
+            step = 0.01;
+            c = pc.getc();
+        }
+        switch (c) {
+            case 'a':
+                speed[_A_]+=step;
+                mortar_A = speed[_A_];
+                if (mortar_A.read() == STOP)    led1=OFF;
+                else                            led1=ON;
+                pc.printf( "mortar_A : %3f \r\n", 1.0-mortar_A.read() );
+                break;
+            case 'A':
+                speed[_A_]-=step;
+                mortar_A = speed[_A_];
+                if (mortar_A.read() == STOP)    led1=OFF;
+                else                            led1=ON;
+                pc.printf( "mortar_A : %3f \r\n", 1.0-mortar_A.read() );
+                break;
+            case 'U':
+                speed[_U_]+=step;
+                mortar_U = speed[_U_];
+                if (mortar_U.read() == STOP)    led2=OFF;
+                else                            led2=ON;
+                pc.printf( "mortar_U : %3f \r\n", mortar_U.read() );
+                pc.printf( "%d \r\n", hight.getPulses() );
+                break;
+            case 'u':
+                speed[_U_]-=step;
+                mortar_U = speed[_U_];
+                if (mortar_U.read() == STOP)    led2=OFF;
+                else                            led2=ON;
+                pc.printf( "mortar_U : %3f \r\n", mortar_U.read() );
+                pc.printf( "%d \r\n", hight.getPulses() );
+                break;
+            case 'm':
+                speed[_M_]+=step;
+                mortar_M = speed[_M_];
+                if (mortar_M.read() == STOP)    led3=OFF;
+                else                            led3=ON;
+                pc.printf( "mortar_M : %3f \r\n", 1.0-mortar_M.read() );
+                break;
+            case 'M' :
+                speed[_M_]-=step;
+                mortar_M = speed[_M_];
+                if (mortar_M.read() == STOP)    led3=OFF;
+                else                            led3=ON;
+                pc.printf( "mortar_M : %3f \r\n", 1.0-mortar_M.read() );
+                break;
+            case 's':
+                speed[_S_]+=step;
+                mortar_S = speed[_S_];
+                if (mortar_S.read() == STOP)    led4=OFF;
+                else                            led4=ON;
+                pc.printf( "mortar_S : %3f \r\n", 1.0-mortar_S.read() );
+                break;
+            case 'S':
+                speed[_S_]-=step;
+                mortar_S = speed[_S_];
+                if (mortar_S.read() == STOP)    led4=OFF;
+                else                            led4=ON;
+                pc.printf( "mortar_S : %3f \r\n", 1.0-mortar_S.read() );
+                break;
+            case 'H':
+                UFO = ON;
+                pc.printf( "UFO : ON \r\n" );
+                break;
+            case 'h':
+                UFO = OFF;
+                pc.printf( "UFO : OFF \r\n" );
+                break;
+            case 'R':
+                RISE1 = UP;
+                pc.printf( "up \r\n" );
+                break;
+            case 'r':
+                RISE1 = DOWN;
+                pc.printf( "down \r\n" );
+                break;
+        }
+    }
+}
+
+void semi() {
+    
+    char c=pc.getc();
+    if(c=='E') {
+        pc.printf( "wait \r\n" );
+        ALLSTOP();
+        pc.printf( "stop \r\n" );
+        pc.printf( "input \r\n" );
+        pc.getc();
+    }
+}
+
+void SEMIAUTO() {
+    
+    char c;
+    
+    pc.printf( "S" );
+    wait(0.5);
+    pc.printf( "semiauto mode \r\n" );
+    while(1) {
+        c = pc.getc();
+        if (c=='b') break;
+        pc.attach(semi, Serial::RxIrq);
+        switch (c) {
+            case 'A':
+                AORI();
+                break;
+            case 'U':
+                egg_hand_rot(egg_gasya_times);
+                break;
+            case 'u':
+                rice_hand_rot(2);
+                break;
+            case 'M':
+                merry();
+                break;
+            case 'S':
+                serve();
+                break;
+        }
+        __disable_irq();
+        pc.printf( "OK \r\n" );
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cook.cpp	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,237 @@
+/* cook.cpp */
+
+#include "global.h"
+
+/**
+ * Parameters
+ */
+int aori_times = 13;
+
+double hand_interval=0.5, egg_gasya_times=6, rice_gasya_times=12;
+int highest=0, lowest=-750, middle=-450;  //rotary encoder
+
+double servetime = 9.0;
+
+/**
+ * Functions
+ */
+void AORI() {
+    
+    int top, count=0;
+    if (MS_A.read()) {
+        top=ON;
+    } else {
+        top=OFF;
+    }
+    
+    setspeed(duty_A, _A_);
+    while(count<aori_times) {
+        if(MS_A.read()!=top) {
+            if(top==OFF) {
+                top=ON;
+                count++;
+            }
+            else {
+                top=OFF;
+            }
+        }
+        wait(0.05);
+    }
+    setspeed(dutyr_A, _A_);
+    while(MS_A.read()) {
+        wait(0.05);
+    }
+    setspeed(STOP, _A_);
+}
+
+void updown(int trg) {
+    
+    if(hight.getPulses()<trg) {
+        setspeed(dutyu_U, _U_);
+        if(trg==highest) {
+            
+        while(MS_U.read()==1 && hight.getPulses()<trg) {
+            wait(0.05);
+        }
+        setspeed(STOP, _U_);
+        
+        }
+        else{
+            while(hight.getPulses()<trg) {
+                wait(0.05);
+            }
+            setspeed(STOP, _U_);
+        }
+    }
+    
+    else if(hight.getPulses()>trg) {
+        setspeed(dutyd_U, _U_);
+        
+        if(trg==highest) {
+            
+        while(MS_U.read()==1 && hight.getPulses()>trg) {
+            wait(0.05);
+        }
+        setspeed(STOP, _U_);
+        
+        }
+        else{
+            while(hight.getPulses()>trg) {
+                wait(0.05);
+            }
+            setspeed(STOP, _U_);
+        }
+    }
+}
+
+void gasya ( short n ) {
+    for( short i=0; i<n; i++ ) {
+        UFO = CLOSE;
+        wait(hand_interval);
+        UFO = OPEN;
+        wait(hand_interval);
+    }
+}
+
+void egg_hand_rot(int n) {
+    
+    UFO = CLOSE;
+    updown(lowest);
+    wait(0.5);
+    gasya(n);
+    wait(0.5);
+    UFO = CLOSE;
+    wait(0.3);
+    updown(highest);
+    //UFO = CLOSE;
+}
+
+void rice_hand_rot(int n) {
+    
+    for (short i=0; i<n; i++) {
+        UFO = CLOSE;
+        updown(lowest);
+        wait(0.3);
+        gasya(rice_gasya_times);
+        UFO = CLOSE;
+        wait(0.3);
+        updown(middle);
+        wait(0.3);
+        UFO = OPEN;
+        wait(1);
+        UFO = CLOSE;
+     }
+     updown(highest);
+}
+
+void merry() {
+    //int count;
+    setspeed(duty_M, _M_);
+    wait(1);
+    while(MS_M.read()) {
+        wait(0.1);
+    }
+    setspeed(STOP, _M_);
+}
+
+void serve() {
+    setspeed(duty_S, _S_);
+    wait(servetime);
+    setspeed(STOP, _S_);
+    wait(4.0);
+    setspeed(dutyr_S, _S_);
+    while(MS_S.read()) {
+        wait(0.05);
+    }
+    setspeed(STOP, _S_);
+}
+
+void cook() {
+    pc.attach(cook_stop, Serial::RxIrq);
+    double gg=0.7;
+    wait(1.0);
+    
+    //merry rice
+    if(STOP_STATUS) {
+        pc.printf( "merry egg \r\n" );
+        merry();
+        wait(gg);
+    }
+    
+    //hand rice
+    if(STOP_STATUS) {
+        pc.printf( "hand \r\n" );
+        egg_hand_rot(egg_gasya_times-2);
+        wait(gg);
+    }
+    
+    //merry egg
+    if(STOP_STATUS) {
+        pc.printf( "merry rice \r\n" );
+        merry();
+        wait(gg);
+    }
+    
+    //hand egg
+    if(STOP_STATUS) {
+        pc.printf( "hand \r\n" );
+        egg_hand_rot(egg_gasya_times*2);
+        //rice_hand_rot(2);
+        wait(gg);
+    }
+    
+    //merry, hand 3
+    if(STOP_STATUS) {
+        pc.printf( "merry 3 \r\n" );
+        merry();
+        wait(gg);
+        pc.printf( "hand \r\n" );
+        egg_hand_rot(egg_gasya_times);
+        //rice_hand_rot(1);
+        wait(gg);
+    }
+    
+    //merry, hand 4
+    if(STOP_STATUS) {
+        pc.printf( "merry 4 \r\n" );
+        merry();
+        wait(gg);
+        pc.printf( "hand \r\n" );
+        egg_hand_rot(egg_gasya_times);
+        //rice_hand_rot(2);
+        wait(gg);
+    }
+    
+    //AORI
+    if(STOP_STATUS) {
+        pc.printf( "aori \r\n" );
+        RISE1 = UP;
+        wait(3.0);
+        AORI();
+        wait(1);
+        RISE1 = DOWN;
+        wait(2);
+        wait(gg);
+    }
+    
+    // serve
+    if(STOP_STATUS) {
+        pc.printf( "serve \r\n" );
+        serve();
+        wait(3.0);
+    }
+    
+    //end processing
+    if(STOP_STATUS) {
+        pc.printf( "finish!! \r\n" );
+    }
+    else if(!STOP_STATUS) {
+        pc.printf( "stop \r\n" );
+        STOP_STATUS = true;
+    }
+    
+    wait(0.2);
+    pc.putc('f');   //bgm stop
+    wait(0.2);
+    __disable_irq();    //stop interrupt
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/global.cpp	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,114 @@
+/* global.cpp */
+
+#include "global.h"
+
+/**
+ * Classes
+ */
+PwmOut mortar_A(p21);   //aori
+PwmOut mortar_U(p22);   //UFO up down
+PwmOut mortar_M(p23);   //merry-go-round
+PwmOut mortar_S(p24);   //yosou
+
+DigitalOut UFO(p6);     //UFO  ON:close, OFF:open
+DigitalOut RISE1(p5);   //rise
+DigitalOut RISE2(p7);
+DigitalOut RISE3(p8);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+DigitalIn MS_A(p20);    //microswitch
+DigitalIn MS_U(p19);
+DigitalIn MS_M(p18);
+DigitalIn MS_S(p17);
+
+QEI hight(p29, p30, NC, 100);
+
+Serial pc(USBTX, USBRX);    //serialport
+
+/**
+ * Parameters
+ */
+double duty_A=0.13, dutyr_A=0.42, dutyu_U=0.30, dutyd_U=0.65, duty_M=0.30, dutyr_M=0.70, duty_S=0.40, dutyr_S=0.60;
+double speed[4] = {STOP, STOP, STOP, STOP};
+double wait_A=0.45, wait_U=0.2, wait_M=0.2, wait_S=0.15;
+
+bool STOP_STATUS=true;  //false:stop
+
+/**
+ * Functions
+ */
+void led_all(int s){
+    if ( s==ON ){
+        led1=led2=led3=led4=ON;
+    }
+    else if ( s==OFF ) {
+        led1=led2=led3=led4=OFF;
+    }
+}
+
+void setspeed( double t_speed, int x ) {
+    double step, waittime;
+    if(speed[x]<t_speed) {
+        while (speed[x]<t_speed) {
+            if(t_speed-speed[x]>0.05)   step = 0.05;
+            else                        step = 0.01;
+            speed[x]+=step;
+            switch(x) {
+                case _A_:
+                    waittime = wait_A;
+                    mortar_A = speed[x];
+                    break;
+                case _U_:
+                    waittime = wait_U;
+                    mortar_U = speed[x];
+                    break;
+                case _M_:
+                    waittime = wait_M;
+                    mortar_M = speed[x];
+                    break;
+                case _S_:
+                    waittime = wait_S;
+                    mortar_S = speed[x];
+                    break;
+            }
+            wait(waittime);
+        }
+    }
+    else {
+        while(speed[x]>t_speed) {
+            if(speed[x]-t_speed>0.05)   step = 0.05;
+            else                        step = 0.01;
+            speed[x]-=step;
+            switch(x) {
+                case _A_:
+                    waittime = wait_A;
+                    mortar_A = speed[x];
+                    break;
+                case _U_:
+                    waittime = wait_U;
+                    mortar_U = speed[x];
+                    break;
+                case _M_:
+                    waittime = wait_M;
+                    mortar_M = speed[x];
+                    break;
+                case _S_:
+                    waittime = wait_S;
+                    mortar_S = speed[x];
+                    break;
+            }
+            wait(waittime);
+        }
+    }
+}
+
+void ALLSTOP() {
+    setspeed(STOP, _A_);
+    setspeed(STOP, _U_);
+    setspeed(STOP, _M_);
+    setspeed(STOP, _S_);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/global.h	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,120 @@
+/* global.h */
+
+#ifndef full_global_H
+#define full_global_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "QEI.h"
+
+/**
+ * Defines
+ */
+#define ON 1
+#define OFF 0
+#define OPEN 0
+#define CLOSE 1
+#define UP 1
+#define DOWN 0
+#define STOP 0.5
+#define _A_ 0
+#define _U_ 1
+#define _M_ 2
+#define _S_ 3
+
+/**
+ * Classes
+ */
+extern PwmOut mortar_A;
+extern PwmOut mortar_U;
+extern PwmOut mortar_M;
+extern PwmOut mortar_S;
+
+extern DigitalOut UFO;
+extern DigitalOut RISE1;
+extern DigitalOut RISE2;
+extern DigitalOut RISE3;
+
+extern DigitalOut led1;
+extern DigitalOut led2;
+extern DigitalOut led3;
+extern DigitalOut led4;
+
+extern DigitalIn MS_A;
+extern DigitalIn MS_U;
+extern DigitalIn MS_M;
+extern DigitalIn MS_S;
+
+extern QEI hight;
+
+extern Serial pc;
+
+/**
+ * Parameters
+ */
+extern double duty_A, dutyr_A, dutyu_U, dutyd_U, duty_M, dutyr_M, duty_S, dutyr_S;
+extern double speed[];
+extern double wait_A, wait_U, wait_M, wait_S;
+
+extern int aori_times;
+
+extern double hand_interval, egg_gasya_times, rice_gasya_times;
+extern int highest, lowest, middle;
+
+extern double servetime;
+
+extern bool STOP_STATUS;
+
+/**
+ * Functions
+ */
+void led_all(int s);
+
+void setspeed( double t_speed, int x );
+
+void ALLSTOP(void);
+
+void AORI(void);
+
+void updown(int trg);
+
+void gasya ( short n );
+
+void egg_hand_time(void);
+
+void rice_hand_time(int n);
+
+void egg_hand_rot(int n);
+
+void rice_hand_rot(int n);
+
+void merry(void);
+
+void serve(void);
+
+void reset_A(void);
+
+void reset_U(void);
+
+void reset_M(void);
+
+void reset_S(void);
+
+void reset(void);
+
+void cook_stop(void);
+
+void cook(void);
+
+void AUTO(void);
+
+void MANUAL(void);
+
+void semi(void);
+
+void SEMIAUTO(void);
+
+
+#endif /* full_global_h */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,53 @@
+/* main.cpp */
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+#include "QEI.h"
+#include "global.h"
+
+/**
+ * main
+ */
+int main() {
+
+    char c;
+    
+    //default value
+    mortar_A.period_us(50);
+    mortar_A = STOP;
+    mortar_U = STOP;
+    mortar_M = STOP;
+    mortar_S = STOP;
+    UFO = CLOSE;
+    RISE1 = RISE2 = RISE3 = DOWN;
+    MS_A.mode(PullUp);
+    MS_U.mode(PullUp);
+    MS_M.mode(PullUp);
+    MS_S.mode(PullUp);
+    
+    pc.putc('I');   //Initialize PC program
+    wait(0.5);
+    
+    AUTO();
+    
+    pc.printf( "\r\n" );
+    wait(0.2);
+    while(1) {
+        c = pc.getc();
+        switch(c) {
+            case 'A':
+                AUTO();
+                break;
+            case 'M':
+                MANUAL();
+                break;
+            case 'S':
+                SEMIAUTO();
+                break;
+        }
+        pc.printf( "\r\n" );
+        wait(0.2);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Mar 05 05:17:45 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/8e73be2a2ac1
\ No newline at end of file