File content as of revision 7:01bb0b0a98f1:

/*PID処理*/
#define RATE    0.052//52
#define PID_BIAS    0.2
#define REFERENCE   180.0
#define MINIMUM     0.0
#define MAXIMUM     359.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
#define PID_CYCLE   0.05   //s

/*Servoなんか*/
#define WAIT_TIME 0.20

/*Serial Mbed間*/
#define ADDRESS 0xAA


/*BusIn sw　入力値*/
#define StartS 0x01
#define Debug1 0x02
#define Debug2 0x04
#define Debug3 0x08
#define Debug12 0x06
#define Debug23 0x0C
#define Kicker 0x10

#define CW 1
#define CCW -1
#define ENTER   0
#define EXIT    1

Serial Mbed(p9,p10);//tx,rx

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
Serial      Motor(p13,p14);//tx,rx
Serial      pc(USBTX,USBRX);


/*Compass PID SetUp*/
PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
Ticker pidupdate;


/*Servo timer */
Timeout STimer;

extern string StringFIN;
extern void array(int,int,int,int);
extern void micon_rx(void);

int speed[4] = {0};            



double SetDegree = 0;

/*Mbed sensor data */
uint8_t IrData[3] = {0},IrNum = 0;
uint8_t PingData[4]={0};
int Compass = 0;
unsigned int CompassDef = 0;
double SetC = 0, InputPID = 180.0, CompassPID = 0.0;

bool WaitFlag = 0;

/* motor driver*/

enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;

/*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  DegreeToVector[5] = {1,-1,-1,1,1};

int wrapDegree[24] = { 0,//
                    30,//15
                    60,//30
                    90,//45
                    130,//60
                    140,//75
                    170,//90
                    170,//105
                    170,//120
                    180,//135
                    200,//150
                    220,//165
                    90, //180 or270
                    130,//195
                    150,//210
                    180,//225
                    175,//240
                    195,//255
                    195,//270
                    240,//285
                    230,//300
                    270,//315
                    290,//330
                    320//345                
                    };
        
    
    
    
    
/*
    |               |
    |               |
    |       o       |
    |               |
    |               |
    |---------------p,q
    
    
*/