Revision 0:19606d41f8c8, committed 2016-09-29

Comitter:

ojityan

Date:

Thu Sep 29 07:37:00 2016 +0000

Commit message:

??????????????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Thu Sep 29 07:37:00 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/TextLCD/#308d188a2d3a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 29 07:37:00 2016 +0000
@@ -0,0 +1,359 @@
+#include "mbed.h"
+#include "TextLCD.h"
+
+#define Center_anL 0.01f
+#define Center_anR 0.01f
+#define Center_anL_lift 0.01f
+#define Center_anR_lift 0.30f
+
+#define Center_an 0.25f
+
+#define spin_num1 0.005
+#define spin_num2 0.008
+/*
+#define lift_CW() mortor1 = 1; mortor2 = 0
+#define lift_CCW() mortor1 = 0; mortor2 = 1
+#define lift_STOP() mortor1 = 0; mortor2 = 0
+*/
+BusOut LEDArray(PTB8, PTB9, PTB10, PTB11, PTE2, PTE3, PTE4, PTE5);
+TextLCD lcd(PTC12, PTC13, PTC16, PTC17, PTA16, PTA17);
+
+DigitalOut led_l(PTB9);
+DigitalOut led_r(PTB11);
+DigitalOut led_c(PTB10);
+
+//USBSerial vcom;
+Timer t;
+
+DigitalOut myled(PTB8);
+
+AnalogIn SENSOR2_(PTB0);//R
+AnalogIn SENSOR1_(PTC2);//L
+AnalogIn SENSOR3_(PTB2);//C
+
+DigitalIn SENSOR3(PTE21);
+
+AnalogIn lift_sensor_R_(PTB3);
+AnalogIn lift_sensor_L_(PTB1);
+
+DigitalOut lift_led_R(PTE2);//R
+DigitalOut lift_led_L(PTB8);//L
+
+DigitalOut spin_led(PTE5);
+DigitalOut time_led(PTE4);
+
+BusOut iL(PTA1,PTA2);
+PwmOut pL(PTD4);
+
+BusOut iR(PTC0,PTC7);
+PwmOut pR(PTA12);
+
+BusOut i_lift(PTC6,PTC5);
+PwmOut p_lift(PTA4);
+
+union {
+    unsigned char ALL;
+    struct {
+        unsigned R    : 1;
+        unsigned L    : 1;
+        unsigned lift : 1;
+    } bits;
+} flags;
+
+void liftmove(void);
+void lift_stop(void);
+double vector(double num);
+void drive_set(int num_L, int num_R, char mode_L, char mode_R);
+void liftmove(void);
+void spin(void);
+
+void drive_set(int num_L, int num_R, char mode_L, char mode_R);
+void MOTOR_A_STOP(void);//左
+void MOTOR_A_GO(void);
+void MOTOR_A_BACK(void);
+void MOTOR_A_BRAKE(void);
+void MOTOR_B_STOP(void);//右
+void MOTOR_B_GO(void);
+void MOTOR_B_BACK(void);
+void MOTOR_B_BRAKE(void);
+void lift_STOP(void);
+void lift_CW(void);
+void lift_CCW(void);
+
+void led_flash(char SENSOR1, char SENSOR2, char SENSOR3, char lift_sensor_L, char lift_sensor_R);
+void past_data(int past);
+void lift_f(char lift_sensor_R, char lift_sensor_L);
+
+int main(void)
+{
+    myled = 0;
+    int past = 7;
+    int mode = 0;
+    char SENSOR1;
+    char SENSOR2;
+    char SENSOR3;
+    char lift_sensor_R;
+    char lift_sensor_L;
+
+    while(1) {
+        //lcd.printf("%3f,%3f,%3f\n%3f,%3f\n",SENSOR1_.read(),SENSOR2_.read(),SENSOR3_.read(),lift_sensor_R_.read(),lift_sensor_L_.read());
+        if(SENSOR1_.read() < Center_an) {
+            SENSOR1 = 0;
+        } else {
+            SENSOR1 = 1;
+        }
+
+        if(SENSOR2_.read() < Center_an) {
+            SENSOR2 = 0;
+        } else {
+            SENSOR2 = 1;
+        }
+
+
+        if(SENSOR3_.read() < Center_an) {
+            SENSOR3 = 0;
+        } else {
+            SENSOR3 = 1;
+        }
+
+        if(lift_sensor_R_.read() < Center_an) {
+            lift_sensor_R = 0;
+        } else {
+            lift_sensor_R = 1;
+        }
+
+        if(lift_sensor_L_.read() < Center_an) {
+            lift_sensor_L = 0;
+        } else {
+            lift_sensor_L = 1;
+        }
+
+        led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+
+
+
+        switch(mode) {
+            case 0:
+            case 3: {
+                if(lift_sensor_R == 0 && !lift_sensor_L) {
+                    liftmove();
+                    mode = 1;
+                }
+                break;
+            }
+            case 1: {
+                if(!lift_sensor_R) {
+                    t.start();
+                    time_led = 1;
+                    mode = 2;
+                }
+                break;
+            }
+            case 2: {
+                if(t.read() > 2.0) {
+                    t.stop();
+                    time_led = 0;
+                    mode = 3;
+                }
+                break;
+            }
+        }
+
+//        lift_f();
+
+        //すべてのセンサが黒の時, モータAが正転, モータBが正転
+        if(SENSOR1 == 0 && SENSOR2 == 0 && SENSOR3 == 0) {
+            MOTOR_A_GO() ;
+            MOTOR_B_GO() ;
+//            past = 7;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //センサ1と2が黒でセンサ3が白の時, モータAとBが両方とも正転.
+        else if(SENSOR1 == 0 && SENSOR2 == 0 &&SENSOR3 == 1) {
+            MOTOR_A_GO();
+            MOTOR_B_GO();
+//            past = 5;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //センサ1と3が黒でセンサ2が白の時, モータAが逆転, モータBが正転.
+        else if(SENSOR1 == 0 && SENSOR2 == 1 && SENSOR3 == 0) {
+            MOTOR_A_BACK() ;
+            MOTOR_B_GO();
+            past = 6;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //センサ1が黒でセンサ2と3が白の時, モータAが停止, モータBが正転.
+        else if(SENSOR1 == 0 && SENSOR2 == 1 && SENSOR3 == 1) {
+            MOTOR_A_STOP() ;
+            MOTOR_B_GO();
+            past = 4;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //センサ1が白でセンサ2と3が黒の時, モータAが正転, モータBが逆転.
+        else if(SENSOR1 == 1 && SENSOR2 == 0 && SENSOR3 == 0) {
+            MOTOR_A_GO() ;
+            MOTOR_B_BACK() ;
+            past = 3;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //センサ1と3が白でセンサ2が黒の時, モータAが正転, モータBが停止.
+        else if(SENSOR1 == 1 && SENSOR2 == 0 && SENSOR3 == 1) {
+            MOTOR_A_GO() ;
+            MOTOR_B_STOP() ;
+            past = 1;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //センサ1と2が白でセンサ３が黒の時, モータAが正転, モータBが正転.
+        else if(SENSOR1 == 1 && SENSOR2 == 1 && SENSOR3 == 0) {
+            MOTOR_A_GO() ;
+            MOTOR_B_GO() ;
+//            past = 2;
+            led_flash(SENSOR1, SENSOR2, SENSOR3, lift_sensor_R, lift_sensor_L);
+        }
+        //すべてのセンサが白の時，過去のデータを使う
+        else if(SENSOR1 == 1 && SENSOR2 == 1 && SENSOR3 == 1) {
+            past_data(past);
+        }
+//        wait(0.05);
+    }
+}
+
+void past_data(int past)//過去のデータを使ってライントレース
+{
+    flags.bits.lift = 1;
+    switch(past) {
+        case 1: {
+            MOTOR_A_GO();
+            MOTOR_B_STOP();
+            break;
+        }
+        case 7:
+        case 5:
+        case 2: {
+            MOTOR_A_GO();
+            MOTOR_B_GO();
+            break;
+        }
+        case 3: {
+            MOTOR_A_GO();
+            MOTOR_B_BACK();
+            break;
+        }
+        case 4: {
+            MOTOR_A_STOP();
+            MOTOR_B_GO();
+            break;
+        }
+        case 6: {
+            MOTOR_A_BACK();
+            MOTOR_B_GO();
+            break;
+        }
+    }
+}
+
+void MOTOR_A_STOP(void)
+{
+    iL = 0;
+    pL = 0.0f;
+}
+void MOTOR_A_GO(void)
+{
+    iL = 1;
+    pL = 0.007f;
+}
+void MOTOR_A_BACK(void)
+{
+    iL = 2;
+    pL = 0.01f;
+}
+
+void MOTOR_B_STOP(void)
+{
+    iR = 0;
+    pR = 0.0f;
+}
+void MOTOR_B_GO(void)
+{
+    iR = 1;
+    pR = 0.007f;
+}
+void MOTOR_B_BACK(void)
+{
+    iR = 2;
+    pR = 0.01f;
+}
+
+void lift_STOP(void)
+{
+    i_lift = 0;
+    p_lift = 0.0f;
+}
+void lift_CW(void)
+{
+    i_lift = 2;
+    p_lift = 0.7f;
+}
+void lift_CCW(void)
+{
+    i_lift = 1;
+    p_lift = 0.7f;
+}
+
+void led_flash(char SENSOR1, char SENSOR2, char SENSOR3, char lift_sensor_L, char lift_sensor_R)//LEDを光らせる
+{
+    led_r = SENSOR1;
+    led_l = SENSOR2;
+    led_c = SENSOR3;
+    lift_led_L = lift_sensor_L;
+    lift_led_R = lift_sensor_R;
+}
+
+
+
+void liftmove(void)//リフトの動作
+{
+    static char mode = 0;
+    if(!mode) {
+        wait(0.3);
+        MOTOR_A_STOP();
+        MOTOR_B_STOP();
+//        spin();
+        wait(1.0);
+        lift_CW();
+        wait(1.5);
+        lift_STOP();
+        mode = 1;
+        MOTOR_A_GO();
+        MOTOR_B_GO();
+        wait(0.5);
+        myled = 1;
+
+    } else {
+        MOTOR_A_STOP();
+        MOTOR_B_STOP();
+        wait(1.0);
+        lift_CCW();
+        wait(0.8);
+        lift_STOP();
+        MOTOR_A_BACK();
+        MOTOR_B_BACK();
+        wait(1.0);
+        MOTOR_A_STOP();
+        MOTOR_B_STOP();
+        myled = 0;
+        while(1) {
+            spin_led = !spin_led;
+            wait(0.1);
+        }
+    }
+}
+void spin(void)//確認用で入るとそこでストップし対応するLEDが点滅する
+{
+    while(1) {
+        MOTOR_A_STOP();
+        MOTOR_B_STOP();
+        spin_led = !spin_led;
+        wait(0.1);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Sep 29 07:37:00 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9296ab0bfc11
\ No newline at end of file