Revision 1:69d85918eaf8, committed 2011-06-26

Comitter:

sunifu

Date:

Sun Jun 26 13:54:14 2011 +0000

Parent:

0:1dbc1f10077d

Commit message:

Changed in this revision

--- a/main.cpp	Sun Jun 26 13:34:01 2011 +0000
+++ b/main.cpp	Sun Jun 26 13:54:14 2011 +0000
@@ -1,104 +1,352 @@
 #include "mbed.h"
 #include "TextLCD.h"
 
-#define THR 200
+#define U_THR 0.65
+#define D_THR 0.2
+#define L_THR 0.2
+#define R_THR 0.65
+#define H_THR 0.6
+#define CH_THR 0.6
 
-enum SignalMode{NOT_USED,FORWARD,BACK,LEFT,RIGHT,DASH_FORWARD,FORWARD_LEFT,FORWARD_RIGHT,DASH_FORWARD_LEFT,
-            DASH_FORWARD_RIGHT,BACK_LEFT,BACK_RIGHT,DASH_BACK,DASH_BACK_LEFT,DASH_BACK_RIGHT, STOP};
-SignalMode STAT;    
+#define HEADER 2
+#define CHA_INTERVAL1 130 // 130
+#define CHA_INTERVAL2 10  // 10
+#define CHB_INTERVAL1 110 // 130
+#define CHB_INTERVAL2 30  // 10
+#define CHC_INTERVAL1 90 // 130
+#define CHC_INTERVAL2 50  // 10
+#define CHD_INTERVAL1 70  // 130
+#define CHD_INTERVAL2 70  // 10
 
-int steering;        
-int signal;
-
-InterruptIn event(p21) ; 
+typedef enum { OFF, ON } state;
 
 TextLCD lcd(p24, p26, p27, p28, p29, p30);
-BusOut myleds(LED1,LED2,LED3,LED4);
+PwmOut out(p21);
+AnalogIn lr_in(p18); // Joystick Left,Right -> Left+Right 
+AnalogIn ud_in(p19); // Joystick Up,Down -> Forward+Backward 
+AnalogIn hs_in(p20); // PSW hs -> Dash!
+AnalogIn ch_in(p17); // PSW ch -> Change CH A,B,C,D
 
-int bitCnt = 0;
-Timer timer;
-int begin,end,interval;
+typedef void (*FUNCPTR)();
+FUNCPTR pFuncBand;
+// Remote Control Signal ON
+void on(){
+    int begin;
+    Timer ton;
+    ton.start();
+    begin = ton.read_us();
+    while ( 500 > ton.read_us() - begin )
+        out.write(0.0f) ;
+    begin = ton.read_us();  
+    while ( 1000 > ton.read_us() - begin )
+        out.write(0.5f) ;
+    ton.stop();
+}
 
-void tStart()
-{
-    interval = 0;
-    begin = timer.read_us();
+// Remote Control Signal OFF
+void off(){
+    int begin;
+    Timer toff;
+    toff.start();
+    begin = toff.read_us();
+    while( 500 > toff.read_us() - begin )
+        out.write(0.0f) ;
+   
+    begin = toff.read_us();
+    while ( 500 > toff.read_us() - begin )
+        out.write(0.5f) ;
+    toff.stop() ;    
 }
 
-void tEnd()
+int flag=0;
+void tail()
 {
-    end = timer.read_us();
-    interval = end - begin ;
-    timer.reset();  
-    
-    if( 2000 - THR < interval && interval < 2000 + THR ){
-        bitCnt = 0 ;
-        signal = 0;
-    }else if ( 500 -THR < interval && interval < 500 + THR ){
-        signal <<= 1;
-        bitCnt++;
-    }else if ( 1000 -THR < interval && interval < 1000 + THR ){
-        signal <<= 1;
-        signal++;
-        bitCnt++;       
-    }   
-   
+    out.write(0.0f) ; 
+    flag = 0;
+}
+
+void bandA(){
+    off();off();
+}
+void bandB(){
+    off();on();    
+}
+void bandC(){
+    on();off();    
+}
+void bandD(){
+    on();on();  
+}       
+
+Ticker in;
+Timer timer;
+float lr, ud, hs ;
+volatile float ch;
+int interval1, interval2;
+
+// Joystick Sensor
+void input()
+{
+    lr = lr_in;
+    ud = ud_in;
+    hs = hs_in; 
+    ch = ch_in ;       
+}
+
+volatile int psw = 0;
+
+void interval(int iTime,state s){
+    float signal;
+    int t;
+    signal = (s==OFF)? 0.0f : 0.5f; 
+    timer.start();
+    t = timer.read_ms();
+    while ( iTime > timer.read_ms() - t )
+        out.write(signal) ;
+    timer.stop();                        
 }
 
 int main() {
-    int data = 0;
-    timer.start() ;
-    event.fall(&tStart) ;
-    event.rise(&tEnd) ;
+    in.attach(&input, 0.05);
+
+    out.period_us(26);
+    lcd.cls() ;
+    lcd.locate(1,1);
+    lcd.printf("BAND [A]");
+    interval1 = CHA_INTERVAL1;
+    interval2 = CHA_INTERVAL2;
+    (pFuncBand) = &bandA;
 
     while(1) {
-        if ( bitCnt == 6){ 
-            data = signal;
-            lcd.locate(0,0);
-            lcd.printf("%3d",data);
-            lcd.locate(4,0);
-            lcd.printf("%d%d",(data&0x20)?1:0,(data&0x10)?1:0);
-            lcd.printf("%d%d%d%d",(data&0x08)?1:0,(data&0x04)?1:0,(data&0x02)?1:0,(data&0x01)?1:0);
-            lcd.locate(0,1);
-
-            if ( data < 16 )
-                lcd.printf("CH A");
-            else if ( data < 32 )
-                lcd.printf("CH B");
-            else if ( data < 48)
-                lcd.printf("CH C");           
-            else if ( data < 64)
-                lcd.printf("CH D");
-
-            steering = (int)(data&0x0F) ;    
-            lcd.locate(5,1);
-            switch(steering){
-                case LEFT:
-                    lcd.printf("LEFT     ");
-                    myleds = 0x01;
+        // #DEBUG printf("UD%4.2f HS%4.2f ",ud,hs);
+        // #DEBUG printf("\tLR%4.2f CH%1d %4.2f \r\n",lr,psw,ch);
+        if( ch >= CH_THR ){
+            while(1){  
+                if(ch < 0.2 ){   
+                    if ( psw < 3 ){
+                        psw++;
+                    }else{ 
+                        psw=0;
+                    }    
                     break;
-                case RIGHT:
-                    lcd.printf("RIGHT    ");
-                    myleds = 0x08;
-                    break;
-                case FORWARD:
-                    lcd.printf("FORWARD");    
-                    break;
-                case BACK:
-                    lcd.printf("BACK     ");
-                    break;
-                case DASH_FORWARD:
-                    lcd.printf("DASH_FOR ");
-                    break;
-                case DASH_BACK:
-                    lcd.printf("DASH_BACK");
-                    break;    
-                case STOP:
-                    lcd.printf("STOP     ");    
-                    wait_ms(50);
-                    lcd.cls();
-                    myleds =0x00;
-            }    
+                }
+            }
+            
+            lcd.locate(1,1);
+            
+            
+            if(psw == 0 ){
+                lcd.printf("BAND [A]"); printf("banda\r\n");
+                (pFuncBand) = &bandA;
+                interval1 = CHA_INTERVAL1;
+                interval2 = CHA_INTERVAL2;        
+            }else if(psw == 1){
+                lcd.printf("BAND [B]");
+                (pFuncBand) = &bandB;
+                interval1 = CHB_INTERVAL1;
+                interval2 = CHB_INTERVAL2;        
+            }else if(psw == 2){;
+                lcd.printf("BAND [C]");
+                pFuncBand = &bandC;
+                interval1 = CHC_INTERVAL1;
+                interval2 = CHC_INTERVAL2;        
+            }else{
+                lcd.printf("BAND [D]"); printf("band D\r\n");
+                pFuncBand = &bandD;
+                interval1 = CHD_INTERVAL1;
+                interval2 = CHD_INTERVAL2;
+            }              
         }
-    }
-}   
+        lcd.locate(0,0);
+        if ( (U_THR < ud && H_THR < hs) && (L_THR < lr && lr < R_THR)){
+            // Forward + Dash!(0101)
+            lcd.printf("Forward+Dash!   ");
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();   
+            off(); on(); off();on(); 
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();     
+            off(); on(); off(); on();
+            tail();
+        }else if((ud < D_THR && H_THR < hs) && (L_THR < lr && lr < R_THR)){
+            // Backward + Dash!(1100)
+            lcd.printf("Backward+Dash!  ");            
+            interval(interval1,OFF); // chA Interval1
+            interval(HEADER,ON); //HEADER 
+            (pFuncBand)();
+            on(); on(); off();off(); // Control Code 1100
+            interval(interval2,OFF); // chA Interval2 
+            interval(HEADER,ON); // HEADER
+            (pFuncBand)();
+            on(); on(); off(); off(); // Control Code 1100               
+            tail();  
+        }else if ((U_THR < ud && H_THR < hs) && (lr < L_THR)){
+            // Forward + Left + Dash!
+            lcd.printf("For+Left+Dash!  ");            
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();      
+            on(); off(); off(); off(); 
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            on(); off(); off(); off();              
+            tail();        
+        }else if((U_THR < ud && H_THR < hs) && (R_THR < lr)){
+            // Forward + Right + Dash!;
+            lcd.printf("For+Right+Dash! ");            
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            on(); off(); off(); on(); 
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            on(); off(); off(); on();
+            tail();                       
+        }else if((ud < D_THR && H_THR < hs) && (lr < L_THR)){
+            // Backward + Left + Dash!
+            lcd.printf("Back+Left+Dash! ");            
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();      
+            on(); on(); off(); on(); 
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();   
+            on(); on(); off(); on();            
+            tail();
+        }else if((ud < D_THR && H_THR < hs) && (R_THR < lr)){
+            // Backward + Right + Dash!
+            lcd.printf("Back+Right+Dash!");            
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            on(); on(); on(); off(); 
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            on(); on(); on(); off();            
+            tail();
+        }else if ( U_THR < ud && lr < L_THR ){
+            // Forward + Left
+            lcd.printf("Forward+Left    ");
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();  
+            off(); on(); on(); off();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            off(); on(); on(); off();            
+            tail();
+        }else if( U_THR < ud && R_THR < lr){
+            // Forward + Right
+            lcd.printf("Forward+Right   ");
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)(); 
+            off(); on(); on(); on();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();   
+            off(); on(); on(); on();
+            tail();                                  
+        }else if( ud < D_THR && lr < L_THR){
+            // Backward+Left(1010)
+            lcd.printf("Backward+Left   ");
+            (pFuncBand)();
+            interval(HEADER,ON);
+            (pFuncBand)();    
+            on(); off(); on(); off();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();     
+            on(); off(); on(); off();
+            tail();                      
+        }else if (ud < D_THR && R_THR < lr){
+            // Backward + Right (1011)
+            lcd.printf("Backward+Right  ");
+            interval(interval1,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)(); 
+            on(); off(); on(); on();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();  
+            on(); off(); on(); on(); 
+            tail();                    
+        }else if((U_THR <= ud)&&(L_THR < lr && lr < R_THR)){
+            // Forward(0001)
+            lcd.printf("Forward         ");            
+            interval(interval1,OFF);    
+            interval(HEADER,ON);
+            (pFuncBand)();   
+            off();off();off();on();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();     
+            off();off();off();on();            
+            tail();
+        }else if((ud <= D_THR)&&(L_THR < lr && lr < R_THR)){
+            // Backward(0010)
+            lcd.printf("Backward        ");
+            interval(interval1,OFF);   
+            interval(HEADER,ON);
+            (pFuncBand)();   
+            off();off();on();off();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();     
+            off();off();on();off();
+            tail();                              
+        }else if(lr < L_THR && (D_THR <= ud && ud <= U_THR)){
+            // Left(0011)
+            lcd.printf("Left <=          ");            
+            interval(interval1,OFF);      
+            interval(HEADER,ON);
+            (pFuncBand)();  
+            off();off();on();on();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();
+            off();off();on();on(); 
+            tail();                      
+        }else if(R_THR < lr && (D_THR <= ud && ud <= U_THR)){
+            // Right(0100)
+            lcd.printf("Right =>        ");            
+            interval(interval1,OFF);      
+            interval(HEADER,ON);
+            (pFuncBand)();
+            off();on();off();off();
+            interval(interval2,OFF);
+            interval(HEADER,ON);
+            (pFuncBand)();
+            off();on();off();off();
+            tail();            
+        }else if((D_THR <= ud && ud <= U_THR) && (L_THR < lr && lr < R_THR)){
+            if ( flag == 0 ){
+                for ( int i = 0; i < 5 ; i++ ){
+                    // STOP(1111);
+                    lcd.locate(0,0);
+                    lcd.printf("Stop            ");            
+                    interval(interval1,OFF);      
+                    interval(HEADER,ON);
+                    (pFuncBand)();   
+                    on();on();on();on();
+                    interval(interval2,OFF);
+                    interval(HEADER,ON);
+                    (pFuncBand)();
+                    on();on();on();on();
+                }
+                flag = 1;
+            }else{
+                out.write(0.0f) ;
+                lcd.locate(0,0);
+                lcd.printf("                ");    
+            }                       
+        }            
+    }        
+}