ooooooooga
Dependencies: ColorSensor1 HMC6352 PID QEI Servo TextLCD mbed
main.h
00001 ///////////////////////////////////////OGATA////////////////////////////////////////// 00002 ////////////////////////////////////////////////////////////////////////////////////// 00003 //センサの数 00004 #define COLOR_NUM 3 00005 00006 //閾値 00007 #define R_THR 65 00008 #define G_THR 65 00009 #define B_THR 65 00010 #define PINR_THR 2000 00011 00012 00013 enum{ 00014 GO, 00015 STOP 00016 }; 00017 00018 //TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7 00019 ColorSensor color0(p20, p17, p18, p19, 10); 00020 ColorSensor color1(p16, p13, p14, p15, 10); 00021 ColorSensor color2(p12, p9, p10, p11, 10); 00022 Serial pc(USBTX, USBRX); // tx, rx 00023 DigitalOut led[4] = {LED1,LED2,LED3,LED4}; 00024 DigitalOut air[2] = {p7,p8}; 00025 00026 00027 Timer color_t[3]; 00028 Timer jump_t; 00029 Ticker interrupt0; 00030 00031 00032 void rivisedate (); 00033 void colorUpdate (); 00034 uint8_t ptm(unsigned sum); 00035 00036 00037 double proportional = 0; 00038 uint16_t com_val = 0; 00039 unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM]; 00040 double rir,rib ; 00041 00042 00043 00044 00045 00046 00047 00048 ///////////////////////////////////////YUSUKE///////////////////////////////////////// 00049 ////////////////////////////////////////////////////////////////////////////////////// 00050 #define RATE 0.1 00051 #define MINIMUM -1440 00052 #define MAXIMUM 1440 00053 00054 #define PID_CYCLE 0.03 //s 00055 #define COMPASS_CYCLE 0.1 00056 00057 #define P_GAIN 1.2 //0.78 //zensin 1.5 00058 #define I_GAIN 1000000000.0 00059 #define D_GAIN 0.0 //0.009 00060 00061 #define PID_BIAS 0.0 00062 #define REFERENCE 0 00063 00064 #define OUT_LIMIT 1000 00065 00066 extern double ultrasonicValue[4]; 00067 extern uint16_t ultrasonicVal[4]; 00068 extern void Ultrasonic(void); 00069 Timer timer2; 00070 Timer ping_t; 00071 00072 HMC6352 compass(p28/*sda*/, p27/*scl*/); 00073 Ticker compassUpdata; 00074 00075 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); 00076 PID controller(1.0, 0.0, 0.0, RATE); 00077 Ticker pidUpdata; 00078 00079 Servo servoR(p25); 00080 Servo servoL(p26); 00081 00082 DigitalIn swR(p29); 00083 DigitalIn swL(p30); 00084 00085 //DigitalOut led1(LED1); 00086 //DigitalOut led2(LED2); 00087 00088 QEI wheel1 (p24/*A*/, p23/*B*/, NC, 624); 00089 QEI wheel2 (p22, p21, NC, 624); 00090 00091 #define Convert_dekaruto(a) ((a+1000.0)/2.0/1000.0) 00092 #define CYCLE 705.0 00093 00094 enum{ 00095 WAIT, 00096 STRAIGHT, 00097 TURN, 00098 COMP 00099 }; 00100 00101 00102 double vl = 0; 00103 double vc = 0; 00104 double vs = 1; 00105 int clwise = 0; 00106 int anclwise = 0; 00107 00108 int enkoda1; 00109 int enkoda2; 00110 00111 int process = 0; 00112 int vc_count = 0; 00113 int mode_comp = 0; 00114 00115 int inputPID = 0;
Generated on Tue Jul 26 2022 22:51:11 by 1.7.2