Ryo Ogata / Mbed 2 deprecated jumpROBO5_0

ooooooooga

Dependencies:   ColorSensor1 HMC6352 PID QEI Servo TextLCD mbed

main.h@0:e6bb0bcba274, 2013-10-01 (annotated)

Committer:
OGA
Date:
Tue Oct 01 07:44:16 2013 +0000
Revision:
0:e6bb0bcba274
oooooga

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:e6bb0bcba274 1 ///////////////////////////////////////OGATA//////////////////////////////////////////
OGA 0:e6bb0bcba274 2 //////////////////////////////////////////////////////////////////////////////////////
OGA 0:e6bb0bcba274 3 //センサの数
OGA 0:e6bb0bcba274 4 #define COLOR_NUM 3
OGA 0:e6bb0bcba274 5
OGA 0:e6bb0bcba274 6 //閾値
OGA 0:e6bb0bcba274 7 #define R_THR 65
OGA 0:e6bb0bcba274 8 #define G_THR 65
OGA 0:e6bb0bcba274 9 #define B_THR 65
OGA 0:e6bb0bcba274 10 #define PINR_THR 2000
OGA 0:e6bb0bcba274 11
OGA 0:e6bb0bcba274 12
OGA 0:e6bb0bcba274 13 enum{
OGA 0:e6bb0bcba274 14 GO,
OGA 0:e6bb0bcba274 15 STOP
OGA 0:e6bb0bcba274 16 };
OGA 0:e6bb0bcba274 17
OGA 0:e6bb0bcba274 18 //TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
OGA 0:e6bb0bcba274 19 ColorSensor color0(p20, p17, p18, p19, 10);
OGA 0:e6bb0bcba274 20 ColorSensor color1(p16, p13, p14, p15, 10);
OGA 0:e6bb0bcba274 21 ColorSensor color2(p12, p9, p10, p11, 10);
OGA 0:e6bb0bcba274 22 Serial pc(USBTX, USBRX); // tx, rx
OGA 0:e6bb0bcba274 23 DigitalOut led[4] = {LED1,LED2,LED3,LED4};
OGA 0:e6bb0bcba274 24 DigitalOut air[2] = {p7,p8};
OGA 0:e6bb0bcba274 25
OGA 0:e6bb0bcba274 26
OGA 0:e6bb0bcba274 27 Timer color_t[3];
OGA 0:e6bb0bcba274 28 Timer jump_t;
OGA 0:e6bb0bcba274 29 Ticker interrupt0;
OGA 0:e6bb0bcba274 30
OGA 0:e6bb0bcba274 31
OGA 0:e6bb0bcba274 32 void rivisedate ();
OGA 0:e6bb0bcba274 33 void colorUpdate ();
OGA 0:e6bb0bcba274 34 uint8_t ptm(unsigned sum);
OGA 0:e6bb0bcba274 35
OGA 0:e6bb0bcba274 36
OGA 0:e6bb0bcba274 37 double proportional = 0;
OGA 0:e6bb0bcba274 38 uint16_t com_val = 0;
OGA 0:e6bb0bcba274 39 unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
OGA 0:e6bb0bcba274 40 double rir,rib ;
OGA 0:e6bb0bcba274 41
OGA 0:e6bb0bcba274 42
OGA 0:e6bb0bcba274 43
OGA 0:e6bb0bcba274 44
OGA 0:e6bb0bcba274 45
OGA 0:e6bb0bcba274 46
OGA 0:e6bb0bcba274 47
OGA 0:e6bb0bcba274 48 ///////////////////////////////////////YUSUKE/////////////////////////////////////////
OGA 0:e6bb0bcba274 49 //////////////////////////////////////////////////////////////////////////////////////
OGA 0:e6bb0bcba274 50 #define RATE 0.1
OGA 0:e6bb0bcba274 51 #define MINIMUM -1440
OGA 0:e6bb0bcba274 52 #define MAXIMUM 1440
OGA 0:e6bb0bcba274 53
OGA 0:e6bb0bcba274 54 #define PID_CYCLE 0.03 //s
OGA 0:e6bb0bcba274 55 #define COMPASS_CYCLE 0.1
OGA 0:e6bb0bcba274 56
OGA 0:e6bb0bcba274 57 #define P_GAIN 1.2 //0.78 //zensin 1.5
OGA 0:e6bb0bcba274 58 #define I_GAIN 1000000000.0
OGA 0:e6bb0bcba274 59 #define D_GAIN 0.0 //0.009
OGA 0:e6bb0bcba274 60
OGA 0:e6bb0bcba274 61 #define PID_BIAS 0.0
OGA 0:e6bb0bcba274 62 #define REFERENCE 0
OGA 0:e6bb0bcba274 63
OGA 0:e6bb0bcba274 64 #define OUT_LIMIT 1000
OGA 0:e6bb0bcba274 65
OGA 0:e6bb0bcba274 66 extern double ultrasonicValue[4];
OGA 0:e6bb0bcba274 67 extern uint16_t ultrasonicVal[4];
OGA 0:e6bb0bcba274 68 extern void Ultrasonic(void);
OGA 0:e6bb0bcba274 69 Timer timer2;
OGA 0:e6bb0bcba274 70 Timer ping_t;
OGA 0:e6bb0bcba274 71
OGA 0:e6bb0bcba274 72 HMC6352 compass(p28/*sda*/, p27/*scl*/);
OGA 0:e6bb0bcba274 73 Ticker compassUpdata;
OGA 0:e6bb0bcba274 74
OGA 0:e6bb0bcba274 75 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
OGA 0:e6bb0bcba274 76 PID controller(1.0, 0.0, 0.0, RATE);
OGA 0:e6bb0bcba274 77 Ticker pidUpdata;
OGA 0:e6bb0bcba274 78
OGA 0:e6bb0bcba274 79 Servo servoR(p25);
OGA 0:e6bb0bcba274 80 Servo servoL(p26);
OGA 0:e6bb0bcba274 81
OGA 0:e6bb0bcba274 82 DigitalIn swR(p29);
OGA 0:e6bb0bcba274 83 DigitalIn swL(p30);
OGA 0:e6bb0bcba274 84
OGA 0:e6bb0bcba274 85 //DigitalOut led1(LED1);
OGA 0:e6bb0bcba274 86 //DigitalOut led2(LED2);
OGA 0:e6bb0bcba274 87
OGA 0:e6bb0bcba274 88 QEI wheel1 (p24/*A*/, p23/*B*/, NC, 624);
OGA 0:e6bb0bcba274 89 QEI wheel2 (p22, p21, NC, 624);
OGA 0:e6bb0bcba274 90
OGA 0:e6bb0bcba274 91 #define Convert_dekaruto(a) ((a+1000.0)/2.0/1000.0)
OGA 0:e6bb0bcba274 92 #define CYCLE 705.0
OGA 0:e6bb0bcba274 93
OGA 0:e6bb0bcba274 94 enum{
OGA 0:e6bb0bcba274 95 WAIT,
OGA 0:e6bb0bcba274 96 STRAIGHT,
OGA 0:e6bb0bcba274 97 TURN,
OGA 0:e6bb0bcba274 98 COMP
OGA 0:e6bb0bcba274 99 };
OGA 0:e6bb0bcba274 100
OGA 0:e6bb0bcba274 101
OGA 0:e6bb0bcba274 102 double vl = 0;
OGA 0:e6bb0bcba274 103 double vc = 0;
OGA 0:e6bb0bcba274 104 double vs = 1;
OGA 0:e6bb0bcba274 105 int clwise = 0;
OGA 0:e6bb0bcba274 106 int anclwise = 0;
OGA 0:e6bb0bcba274 107
OGA 0:e6bb0bcba274 108 int enkoda1;
OGA 0:e6bb0bcba274 109 int enkoda2;
OGA 0:e6bb0bcba274 110
OGA 0:e6bb0bcba274 111 int process = 0;
OGA 0:e6bb0bcba274 112 int vc_count = 0;
OGA 0:e6bb0bcba274 113 int mode_comp = 0;
OGA 0:e6bb0bcba274 114
OGA 0:e6bb0bcba274 115 int inputPID = 0;