void BWS_and_Compliance_drive(void)
{
float RPS;
float Dev_Position,Dev_Force;
int ch,AD1,AD2,MaxCounter1,MaxCounter2,MinCounter1,MinCounter2;
// int lead=5(BWS),10(Compliance); //ボールねじのリード
// int slit=2000; //エンコーダの分解能
// Reduction_Ratio=12.3;
Counters_Read(); //カウンタ取得
AD1 = AD_Read(1);
AD2 = AD_Read(2); //ロードセルよりADにてデジタル値取得
M[1].Force = AD1/2047.0*1000.0; //力［N］の現在値に換算
M[2].Force = AD2/2047.0*1000.0;
/*ここからコンプライアンス*/
M[1].Target_Position = -M[1].Compliance * M[1].Force; //目標位置［mm］算出
M[1].Position = M[1].Counter*10/2000/Reduction_Ratio; //現在位置［mm］取得
Dev_Position = M[1].Target_Position-M[1].Position; //位置偏差［mm］
MaxCounter1 = M[1].Target_Range*2000*Reduction_Ratio/10; //パルス上限
MinCounter1 = -M[1].Target_Range*2000*Reduction_Ratio/10; //パルス下限
M[1].Input_RPM = M[1].Gain * Dev_Position; //位置フィードバックゲインから目標速度算出
if((-0.1<=Dev_Position)&&(Dev_Position<=0.1)) M[1].Input_RPM = 0;
if(M[1].Counter < MinCounter1) M[1].Input_RPM = 0;
if(MaxCounter1 < M[1].Counter) M[1].Input_RPM = 0;
/*ここから定荷重免荷*/
Dev_Force = M[2].Target_Force - M[ch].Force; //力の偏差を計算
MaxCounter2 = M[2].Target_Range*2000*Reduction_Ratio/5; //パルスの範囲
MinCounter2 = -M[2].Target_Range*2000*Reduction_Ratio/5;
if(Dev_Force > 0) M[2].Input_RPM = M[1].Gain1 * Dev_Force; //のぼり下りでゲインがちがう
else M[2].Input_RPM = M[2].Gain2 * Dev_Force;
if(M[2].Counter < MinCounter2) M[2].Input_RPM = 0;
if(MaxCounter2 < M[2].Counter) M[2].Input_RPM = 0;
if((-50<=AD2)&&(AD2<=50)) M[ch].Input_RPM = 0;
/*同時制御*/
for(ch=1; ch<=Channel; ch++){
M[ch].DA_Value1 = (M[ch].Input_RPM*0.003+2.5057)/5.0*4095.0; //デジタル値に変換
if(M[ch].DA_Value1 >= 4095) M[ch].DA_Value1=4095;
if(M[ch].DA_Value1 <= 0) M[ch].DA_Value1=0;
DA_Write(ch,M[ch].DA_Value1);
M[ch].RPS = (M[ch].Counter-M[ch].Counter_Past)/(2000.0*Reduction_Ratio)/(Samp_Cycle/1000.0);
}
M[1].Velocity = M[ch].RPS*10;
M[2].Velocity = M[ch].RPS*5;
BWS_and_Compliance_Log();
M[1].Counter_Past = M[1].Counter;
M[2].Counter_Past = M[2].Counter;
}
//*** センサ総合 *********************************************************
void Sensors_Display(void)
{
int ch;
Gotoxy(0,6);
Setcolor(GREEN,BLACK);
Counters_Read();
for(ch=1; ch<=Channel; ch++){
printf(" ch%d Counter :%8.3f[mm] (%6d[pulse])\n",ch ,M[ch].Counter/2000.0*5.0/Reduction_Ratio ,M[ch].Counter);
} //カウンタ×４逓倍
for(ch=1; ch<=Channel; ch++){
M[ch].Force = AD_Read(ch)/2047.0*1000.0;
printf(" ch%d Load : %6d (%6.3f[N])\n", ch, AD_Read(ch), M[ch].Force);
}
printf("\n");
Restore();
}
void Force_Const_Drive2(int tekitou)
{
double RPS1,RPS2;
double Dev_Force1,Dev_Force2;
int AD1,AD2;
AD1 = AD_Read(1);
AD2 = AD_Read(2);
M[1].Counter = Count_Read(1);
M[2].Counter = Count_Read(2);
M[1].Force = AD1/2047.0*1000.0; //力の現在値取得
M[2].Force = AD2/2047.0*1000.0;
Dev_Force1 = (M[1].Target_Force - (M[1].Force+M[2].Force)/2);
Dev_Force2 = (M[2].Target_Force - (M[1].Force+M[2].Force)/2); //力の偏差を計算
if(Dev_Force1 > 0) M[1].Input_RPM = M[1].Gain1 * Dev_Force1; //のぼり下りでゲインがちがう
else M[1].Input_RPM = M[1].Gain2 * Dev_Force1;
if(Dev_Force2 > 0) M[2].Input_RPM = M[2].Gain1 * Dev_Force2; //のぼり下りでゲインがちがう
else M[2].Input_RPM = M[2].Gain2 * Dev_Force2;
if((-50<=AD1)&&(AD1<=50)) M[1].Input_RPM = 0;
if((-50<=AD2)&&(AD2<=50)) M[2].Input_RPM = 0;
M[1].DA_Value1 = (M[1].Input_RPM*0.003+2.5057)/5.0*4095.0; //デジタル値に変換
if(M[1].DA_Value1 >= 4095) M[1].DA_Value1=4095;
if(M[1].DA_Value1 <= 0) M[1].DA_Value1=0;
DA_Write(1,M[1].DA_Value1);
M[2].DA_Value1 = (M[2].Input_RPM*0.003+2.5057)/5.0*4095.0; //デジタル値に変換
if(M[2].DA_Value1 >= 4095) M[2].DA_Value1=4095;
if(M[2].DA_Value1 <= 0) M[2].DA_Value1=0;
DA_Write(2,M[2].DA_Value1);
RPS1 = (M[1].Counter-M[1].Counter_Past)/(2000.0*Reduction_Ratio)/(Samp_Cycle/1000.0);
M[1].RPM = RPS1 * 60.0;
RPS2 = (M[2].Counter-M[2].Counter_Past)/(2000.0*Reduction_Ratio)/(Samp_Cycle/1000.0);
M[2].RPM = RPS2 * 60.0;
Force_Const_Log2();
M[1].Counter_Past = M[1].Counter;
M[2].Counter_Past = M[2].Counter;
}
USHORT Fn_CDSsampleOneShot(BOOL* flag)
{
USHORT buffer=0;
AD_SelectADChannel(AD_INPUT_CHANNEL1);
AD_Start();
while((*flag)==0);//while(!flag_ADfinish);
AD_Read(&buffer);
AD_Stop();
*flag=0;
return buffer;
}
int AD_Average(int ch) //AD1000回平均
{
int i;
int AD1;
int AD2 = 0;
int data;
for(i=1; i<=1000; i++){
AD1 = AD_Read(ch);
AD2 = AD2 + AD1;
}
data = AD2/1000;
return data;
}
void Force_Const_Drive(int ch)
{
double RPS;
double Dev_Force;
int AD;
int Dev_Counter;
int MaxCounter;
int MinCounter;
// int lead=5; //ボールねじのリード
// int slit=2000; //エンコーダの分解能
//#define Reduction_Ratio 12.3
AD = AD_Read(ch); //ロードセルから初期値（電圧？）を取得
M[ch].Counter = Count_Read(ch); //カウンタの初期位置取得
Dev_Counter = M[ch].Counter; //カウンタの初期値からの差分
MaxCounter = M[ch].Target_Range*2000*Reduction_Ratio/5; //パルスの範囲
MinCounter = -M[ch].Target_Range*2000*Reduction_Ratio/5;
M[ch].Force = AD/2047.0*1000.0; //力の現在値取得
Dev_Force = M[ch].Target_Force - M[ch].Force; //力の偏差を計算
if(Dev_Force > 0) M[ch].Input_RPM = M[ch].Gain1 * Dev_Force; //のぼり下りでゲインがちがう
else M[ch].Input_RPM = M[ch].Gain2 * Dev_Force;
if(Dev_Counter < MinCounter) M[ch].Input_RPM = 0;
if(MaxCounter < Dev_Counter) M[ch].Input_RPM = 0;
if((-50<=AD)&&(AD<=50)) M[ch].Input_RPM = 0;
M[ch].DA_Value1 = (M[ch].Input_RPM*0.003+2.5057)/5.0*4095.0; //デジタル値に変換
if(M[ch].DA_Value1 >= 4095) M[ch].DA_Value1=4095;
if(M[ch].DA_Value1 <= 0) M[ch].DA_Value1=0;
DA_Write(ch,M[ch].DA_Value1);
RPS = (M[ch].Counter-M[ch].Counter_Past)/(2000.0*Reduction_Ratio)/(Samp_Cycle/1000.0);
M[ch].RPM = RPS * 60.0;
Force_Const_Log(ch);
M[ch].Counter_Past = MaxCounter;
}
BOOL Fn_ADsampleOneShot(BOOL* flag)
{
USHORT buffer=0;
AD_SelectADChannel(AD_INPUT_CHANNEL2);
AD_Start();
while((*flag)==0);//while(!flag_ADfinish);
AD_Read(&buffer);
AD_Stop();
*flag=0;
#ifdef Supply3V
#ifdef NotRailToRail
if(buffer>=0x24E||buffer<=0x118) //"LP273" project using [email protected](actual @2.8V cuz not rail to rail)
return 1; //cuz [email protected]/2=1.4 0x118=0.9V(1.4-0.5) 0x24E=1.9v(1.4+0.5)
else return 0;
#endif
#ifdef RailToRail
if(buffer>=0x294||buffer<=0x168) //"LED tubo" project using [email protected] rail-to-rail [email protected]/2=1.6V 0x294=2.1V 0x168=1.1V
return 1;
else return 0;
#endif
#endif
#ifdef Supply5V
#ifdef NotRailToRail
if(buffer>=0x233||buffer<=0x166)
return 1;
else return 0;
#endif
#ifdef RailToRail
if(buffer>=0x266||buffer<=0x19a)
return 1;
else return 0;
#endif
#endif
}
