匡尚 青柳
ライントレース
Diff: main.cpp
- Revision:
- 0:19606d41f8c8
--- /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);
+ }
+}