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?
User | Revision | Line number | New 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; |