本番(11/10)で使用したプログラム

Dependencies:   MotorDriver_SU USBDevice mbed

Files at this revision

API Documentation at this revision

Comitter:
12f2027a
Date:
Mon Nov 10 14:47:08 2014 +0000
Commit message:
termB 1??????11/10???????????

Changed in this revision

MotorDriver_SU.lib Show annotated file Show diff for this revision Revisions of this file
USBDevice.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
termB-1.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver_SU.lib	Mon Nov 10 14:47:08 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/NT32/code/MotorDriver_SU/#e6c391eb8fac
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice.lib	Mon Nov 10 14:47:08 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/USBDevice/#4d3e7f3d5211
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 10 14:47:08 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/termB-1.cpp	Mon Nov 10 14:47:08 2014 +0000
@@ -0,0 +1,428 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+
+
+DigitalIn sw(P0_1, PullUp);
+DigitalOut led(P1_28);
+AnalogIn   sw1(P0_22);
+AnalogIn   sw2(P0_12);
+AnalogIn   sw3(P0_14);
+AnalogIn   sw4(P0_11);
+AnalogIn   sw5(P0_13);
+
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+
+
+int main( ){
+    float hoge1;
+    float hoge2;
+    float hoge3;
+    float hoge4;
+    float hoge5;
+    int AI = 0;
+     
+        
+        /********************************/
+        /********************************/
+        /********************************/
+        /*荷物を持ち上げる前のプログラム*/
+        /********************************/
+        /********************************/
+        /********************************/
+        
+        while(1){
+        
+        hoge1 = sw1.read();
+        hoge2 = sw2.read();
+        hoge3 = sw3.read();
+        hoge4 = sw4.read();
+        hoge5 = sw5.read();
+        
+        
+        /************************************/
+        /*ラインに沿って走るためのプログラム*/
+        /************************************/
+        ///////////まっすぐ走るためのプログラム//////////
+        /*1.1.1 センサ1が黒*/
+        if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4  > 0.5 && sw5 > 0.5){     
+            motor.Drive(0, CW, 500);           
+            motor.Drive(1, CW, 4095);             
+            
+        }
+        
+        
+        /*1.1.2 センサ2が黒*/
+        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 800);           
+            motor.Drive(1, CW, 4095);           
+            
+        }
+        
+        
+        /*1.1.3 センサ3が黒*/
+        else if(sw1  > 0.5  && sw2  > 0.5 && sw3 <= 0.3 && sw4  > 0.5 && sw5 > 0.5){  
+            motor.Drive(0, CW, 4095);        
+            motor.Drive(1, CW, 4095);      
+            
+        }
+        
+        
+        /*1.1.4 センサ4が黒*/
+        else if(sw1  > 0.5 && sw2  > 0.5 && sw3  > 0.5 && sw4 <= 0.5 && sw5 > 0.5){  
+            motor.Drive(0, CW, 4095);       
+            motor.Drive(1, CW, 800);           
+            
+        }
+        
+        
+        /*1.1.5 センサ5が黒*/
+        else if(sw1  > 0.5 && sw2  > 0.5 && sw3  > 0.5 && sw4  > 0.5 && sw5 <= 0.3){  
+            motor.Drive(0, CW, 4095);        
+            motor.Drive(1, CW, 500);       
+        
+        }
+        
+        /*1.1.6 センサ2-3が黒*/
+        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 1500);         
+            motor.Drive(1, CW, 4095);        
+            
+        }
+        
+        /*1.1.7 センサ3-4が黒*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 4095);         
+            motor.Drive(1, CW, 1500);        
+            
+        }
+        
+        ///////////大きい右カーブを曲がるためのプログラム//////////
+        /*1.2.1 センサ2-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5  <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2.2 センサ3-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 &&  sw4 > 0.5 && sw5  <= 0.3){   
+            AI = 1;
+        }
+        /*1.2.3 センサ4-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 &&  sw4 <= 0.3 && sw5  <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2..3 センサ2-3-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2.4 センサ3-4-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2.5 全部白かつAI = 1*/
+        else if(AI == 1 && sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 4095);         
+            motor.Drive(1, CCW, 4095);        
+            
+        }
+        
+        ///////////大きい左カーブを曲がるためのプログラム//////////
+        /*1.3.1 センサ1-2が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.2 センサ1-3が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.3 センサ1-4が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.4 センサ1-2-3が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.5 センサ1-3が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.6 全部白かつAI = 2*/
+        else if(AI == 2 && sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CCW, 4095);         
+            motor.Drive(1, CW, 4095);        
+            
+        }
+        
+        /**********************************/
+        /*停止位置で止まるためのプログラム*/
+        /**********************************/
+        
+        /*1.4.1 センサ1-3-5が黒*/
+        else if(sw1  <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4  > 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+            
+        }
+        
+        /*1.4.2 センサ1-2-5が黒*/
+        else if(sw1  <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4  > 0.5 && sw5 <= 0.5){
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+            
+        }
+        
+        /*1.4.3 センサ1-4-5が黒*/
+        else if(sw1  <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4  <= 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+        }
+        
+        /*1.4.4 センサ1-2-3-5が黒*/
+        else if(sw1  <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4  > 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+        }
+        
+        /*1.4.5 センサ1-3-4-5が黒*/
+        else if(sw1  <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4  <= 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+        }
+        }
+        
+        ///////////停止//////////
+        motor.Drive(0, STOP, 0);           
+        motor.Drive(1, STOP, 0);
+        wait(0.1);
+        ///////////リフト上げる//////////
+        lift.Drive(0, CCW, 4095);
+        wait(0.8);
+        ///////////リフト停止//////////
+        lift.Drive(0, STOP, 0);
+        wait(0.1);
+        ///////////停止位置を超えるために少し前進//////////
+        motor.Drive(0, CW, 4095);           
+        motor.Drive(1, CW, 4095);
+        wait(1.0);
+        
+        
+        
+        
+        /********************************/
+        /********************************/
+        /********************************/
+        /*荷物を持ち上げた後のプログラム*/
+        /********************************/
+        /********************************/
+        /********************************/
+        
+        
+        while(1){
+        
+        hoge1 = sw1.read();
+        hoge2 = sw2.read();
+        hoge3 = sw3.read();
+        hoge4 = sw4.read();
+        hoge5 = sw5.read();
+        
+        
+        /************************************/
+        /*ラインに沿って走るためのプログラム*/
+        /************************************/
+        ///////////まっすぐ走るためのプログラム//////////
+        /*1.1.1 センサ1が黒*/
+        if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4  > 0.5 && sw5 > 0.5){     
+            motor.Drive(0, CW, 500);           
+            motor.Drive(1, CW, 4095);             
+            
+        }
+        
+        
+        /*1.1.2 センサ2が黒*/
+        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 800);           
+            motor.Drive(1, CW, 4095);           
+            
+        }
+        
+        
+        /*1.1.3 センサ3が黒*/
+        else if(sw1  > 0.5  && sw2  > 0.5 && sw3 <= 0.3 && sw4  > 0.5 && sw5 > 0.5){  
+            motor.Drive(0, CW, 4095);        
+            motor.Drive(1, CW, 4095);      
+            
+        }
+        
+        
+        /*1.1.4 センサ4が黒*/
+        else if(sw1  > 0.5 && sw2  > 0.5 && sw3  > 0.5 && sw4 <= 0.5 && sw5 > 0.5){  
+            motor.Drive(0, CW, 4095);       
+            motor.Drive(1, CW, 800);           
+            
+        }
+        
+        
+        /*1.1.5 センサ5が黒*/
+        else if(sw1  > 0.5 && sw2  > 0.5 && sw3  > 0.5 && sw4  > 0.5 && sw5 <= 0.3){  
+            motor.Drive(0, CW, 4095);        
+            motor.Drive(1, CW, 500);       
+        
+        }
+        
+        /*1.1.6 センサ2-3が黒*/
+        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 1500);         
+            motor.Drive(1, CW, 4095);        
+            
+        }
+        
+        /*1.1.7 センサ3-4が黒*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 4095);         
+            motor.Drive(1, CW, 1500);        
+            
+        }
+        
+        ///////////大きい右カーブを曲がるためのプログラム//////////
+        /*1.2.1 センサ2-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5  <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2.2 センサ3-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 &&  sw4 > 0.5 && sw5  <= 0.3){   
+            AI = 1;
+        }
+        /*1.2.3 センサ4-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 &&  sw4 <= 0.3 && sw5  <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2..3 センサ2-3-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2.4 センサ3-4-5が黒(センサ5によって右のカーブであることを判定する)*/
+        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){   
+            AI = 1;
+        }
+        
+        /*1.2.5 全部白かつAI = 1*/
+        else if(AI == 1 && sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CW, 4095);         
+            motor.Drive(1, CCW, 4095);        
+            
+        }
+        
+        ///////////大きい左カーブを曲がるためのプログラム//////////
+        /*1.3.1 センサ1-2が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.2 センサ1-3が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.3 センサ1-4が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.4 センサ1-2-3が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.5 センサ1-3が黒(センサ1によって右のカーブであることを判定する)*/
+        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
+            AI = 2;
+        }
+        
+        /*1.3.6 全部白かつAI = 2*/
+        else if(AI == 2 && sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
+            motor.Drive(0, CCW, 4095);         
+            motor.Drive(1, CW, 4095);        
+            
+        }
+        
+        /**********************************/
+        /*停止位置で止まるためのプログラム*/
+        /**********************************/
+        
+        /*1.4.1 センサ1-3-5が黒*/
+        else if(sw1  <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4  > 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+            
+        }
+        
+        /*1.4.2 センサ1-2-5が黒*/
+        else if(sw1  <= 0.5 && sw2 <= 0.5 && sw3 > 0.5 && sw4  > 0.5 && sw5 <= 0.5){
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+            
+        }
+        
+        /*1.4.3 センサ1-4-5が黒*/
+        else if(sw1  <= 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4  <= 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+        }
+        
+        /*1.4.4 センサ1-2-3-5が黒*/
+        else if(sw1  <= 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4  > 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+        }
+        
+        /*1.4.5 センサ1-3-4-5が黒*/
+        else if(sw1  <= 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4  <= 0.5 && sw5 <= 0.5){     
+            motor.Drive(0, STOP, 0);           
+            motor.Drive(1, STOP, 0);             
+            break;
+        }
+        }
+        
+        ///////////停止//////////
+        motor.Drive(0, STOP, 0);           
+        motor.Drive(1, STOP, 0);
+        wait(0.1);
+        ///////////リフト下げる//////////
+        lift.Drive(0, CW, 4095);
+        wait(1.0);
+        ///////////リフト停止//////////
+        lift.Drive(0, STOP, 0);
+        wait(0.1);
+        ///////////バック//////////
+        motor.Drive(0, CCW, 4095);           
+        motor.Drive(1, CCW, 4095);
+        wait(3.0);
+        ///////////モータ停止//////////
+        motor.Drive(0, STOP, 0);           
+        motor.Drive(1, STOP, 0);
+        wait(1.0);
+        
+        
+
+}
\ No newline at end of file