Diff: mainh.txt

Revision:

3:2f74791564c9

Parent:

2:39135c67083d

diff -r 39135c67083d -r 2f74791564c9 mainh.txt
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mainh.txt	Wed Mar 11 07:47:19 2015 +0000
@@ -0,0 +1,166 @@
+
+/*PID処理*/
+#define RATE        0.052//52
+#define PID_BIAS    0.3
+#define REFERENCE   180.0
+#define MINIMUM     0.1
+#define MAXIMUM     360.0
+#define P_GAIN      1.4//1.4    //0.78   
+#define I_GAIN      0.0     //0.0
+#define D_GAIN      0.05  //0.009
+#define OUT_LIMIT   40.0
+#define MAX_POW     100
+#define MIN_POW     -100
+
+/*Serial Mbed間*/
+#define ADDRESS 0xAA
+#define DATA_NUM 9
+
+
+/*BusIn sw　入力値*/
+#define StartS 0x01
+#define Debug1 0x02
+#define Debug2 0x04
+#define Debug3 0x08
+#define Kicker 0x10
+
+#define CW 1
+#define CCW -1
+#define ENTER   0
+#define EXIT    1
+
+
+
+BusIn   Sw(p22,p23,p24,p25,p26);
+BusIn   Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
+DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};
+DigitalOut  Kick(p29);
+Servo       S555(p21);
+AQM0802A    Lcd(p28,p27);//sda,scl
+MultiSerial Mbed(p9,p10);//tx,rx
+Serial      Motor(p13,p14);//tx,rx
+Serial      pc(USBTX,USBRX);
+
+Ticker  Monitor;
+
+
+/*Compass PID SetUp*/
+PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
+
+/* motor driver*/
+extern string StringFIN;
+extern void array(int,int,int,int);
+int speed[4] = {0};
+
+  
+/*PID補正move加算値 Revise */
+double ReV = 0.0;
+
+/*Compass*/
+int CompassDef = 0;
+double SetAngle = 0; 
+
+enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
+
+int SetRad = 0;
+
+/*StartLoop*/
+bool loop;
+
+/*Mbed sensor data */
+uint8_t IrData[3] = {0},IrNum = 0;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
+uint8_t PingData[4]={0};
+unsigned int Compass = 0;
+
+uint8_t rx_data[DATA_NUM];
+/*mbed sensor data */
+
+volatile  int Angle[180] = {
+    0   ,   1   ,   2   ,   3   ,
+    4   ,   5   ,   6   ,   7   ,
+    8   ,   9   ,   10  ,   11  ,
+    12  ,   13  ,   14  ,   15  ,
+    16  ,   17  ,   18  ,   19  ,
+    20  ,   21  ,   22  ,   23  ,
+    24  ,   25  ,   26  ,   27  ,
+    28  ,   29  ,   30  ,   31  ,
+    32  ,   33  ,   34  ,   35  ,
+    36  ,   37  ,   38  ,   39  ,
+    40  ,   41  ,   42  ,   43  ,
+    44  ,   45  ,   46  ,   47  ,
+    48  ,   49  ,   50  ,   51  ,
+    52  ,   53  ,   54  ,   55  ,
+    56  ,   57  ,   58  ,   59  ,
+    60  ,   61  ,   62  ,   63  ,
+    64  ,   65  ,   66  ,   67  ,
+    68  ,   69  ,   70  ,   71  ,
+    72  ,   73  ,   74  ,   75  ,
+    76  ,   77  ,   78  ,   79  ,
+    80  ,   81  ,   82  ,   83  ,
+    84  ,   85  ,   86  ,   87  ,
+    88  ,   89  ,   90  ,   -89 ,
+    -88 ,   -87 ,   -86 ,   -85 ,
+    -84 ,   -83 ,   -82 ,   -81 ,
+    -80 ,   -79 ,   -78 ,   -77 ,
+    -76 ,   -75 ,   -74 ,   -73 ,
+    -72 ,   -71 ,   -70 ,   -69 ,
+    -68 ,   -67 ,   -66 ,   -65 ,
+    -64 ,   -63 ,   -62 ,   -61 ,
+    -60 ,   -59 ,   -58 ,   -57 ,
+    -56 ,   -55 ,   -54 ,   -53 ,
+    -52 ,   -51 ,   -50 ,   -49 ,
+    -48 ,   -47 ,   -46 ,   -45 ,
+    -44 ,   -43 ,   -42 ,   -41 ,
+    -40 ,   -39 ,   -38 ,   -37 ,
+    -36 ,   -35 ,   -34 ,   -33 ,
+    -32 ,   -31 ,   -30 ,   -29 ,
+    -28 ,   -27 ,   -26 ,   -25 ,
+    -24 ,   -23 ,   -22 ,   -21 ,
+    -20 ,   -19 ,   -18 ,   -17 ,
+    -16 ,   -15 ,   -14 ,   -13 ,
+    -12 ,   -11 ,   -10 ,   -9  ,
+    -8  ,   -7  ,   -6  ,   -5  ,
+    -4  ,   -3  ,   -2  ,   -1  };
+
+volatile int  RadToVector[4] = {1,-1,-1,1};
+
+int wrapRad[24] = { 0,//
+                    15,//15
+                    60,//30
+                    80,//45
+                    100,//60
+                    110,//75
+                    180,//90
+                    180,//105
+                    185,//120
+                    230,//135
+                    250,//150
+                    260,//165
+                    90, //180 or270
+                    100,//195
+                    110,//210
+                    130,//225
+                    175,//240
+                    180,//255
+                    180,//270
+                    250,//285
+                    260,//300
+                    280,//315
+                    300,//330
+                    345//345                
+                    };
+        
+    
+    
+    
+    
+/*
+    |               |
+    |               |
+    |       o       |
+    |               |
+    |               |
+    |---------------p,q
+    
+    
+*/
\ No newline at end of file