File content as of revision 14:3403ce49a37a:

/*
// pin番号・変数を定義
// pid関数のプロトタイプ宣言

//encoder
InterruptIn a(PA_13);
InterruptIn b(PA_14);
InterruptIn a2(PC_10);
InterruptIn b2(PC_12);
//arm_motor
PwmOut bole_f(PB_6);
PwmOut bole_b(PC_9);
PwmOut bloss_f(PB_5);
PwmOut bloss_b(PA_10);
//hand_motor
PwmOut catch1_f(PC_7);
PwmOut catch1_b(PB_3);
PwmOut catch2_f(PB_10);
PwmOut catch2_b(PB_4);

//hand_valve
DigitalOut valve1(PC_4);
//hand_touch-sensor
DigitalIn sw1(PA_11);
DigitalIn sw2(PC_2);
//bole_find-sensor
AnalogIn IR_r(PC_0);
AnalogIn IR_l(PC_3);
DigitalInOut uw(PB_0);


Serial pc(USBTX, USBRX);
*/

//variable
long int count=0,count2=0,pa,pb,pa2,pb2;

bool c0=0,c1=0,c2=0,c3=0;

double radius=14.4;    //回転半径[mm]
double gear1=16.0/36.0;//boleギア比
double gear2=20.0/36.0;//blossギア比

float dist_n=0.0f;     //現在の距離[mm]
float dist_p=0.0f;     //過去の距離[mm]

float devi=0.0f;       //目標との差
float diff=0.0f;       //微分値
float inte=0.0f;       //積算量
float dt=0.0f;         //割り込み時間
float pwm=0.0f;        //操作量
int ratio=1000.0f;     //出力値調整

int n=0;        //幹検知回数カウント
int target=0;   //目標値:高さ[mm]（確認/調整用）

double dist;    //幹との距離[mm]
bool UW;        //超音波の判断

int r,l;        //幹の有無検知
bool IR;        //赤外線の判断


bool sw_state=0;//タッチセンサの判断
bool sw1_n=0;    //現在のタッチセンサ１の状態
bool sw1_p=0;    //過去のタッチセンサ１の状態
bool sw2_n=0;    //現在のタッチセンサ２の状態
bool sw2_p=0;    //過去のタッチセンサ２の状態


//Timer-setup
Timer meas_pulse;
Timer count_time;
//Ticker-setup
Ticker bole_checker;
Ticker sw_check;



//pid-prototype
void pid(float t,int count,double rate,bool c = 0);