//定荷重免荷と左右方向のコンプライアンスを同時制御
void BWS_and_Compliance(void)
{
int Flag=1;
int yes;
int ch;
while(Flag){
Display_Title(); //タイトル表示
printf(" <BWS_and_Compliance_test>\n"); //サブタイトル表示
Sensors_Display();
printf("\n Input Target Force (BWS) (0 - 630)[N]) >> "); //免荷力入力
M[2].Target_Force = Double_Stdin(0,630);
printf("\n Input Gain (horizontal) (0 - 1000)[rpm/N]) >> ");
M[1].Gain1 = Double_Stdin(0,1000);
printf("\n Input Gain (UP) (0 - 1000)[rpm/N]) >> "); //ゲイン入力（のぼり(ch1)）
M[2].Gain1 = Double_Stdin(0,1000);
printf("\n Input Gain (DOWN) (0 - 1000)[rpm/N]) >> "); //ゲイン入力（くだり(ch2)）
M[2].Gain2 = Double_Stdin(0,1000);
printf("\n Input Target Range (horizontal) (0 - 100)[mm]) >> "); //片側可動域入力
M[1].Target_Range = Double_Stdin(0,100);
printf("\n Input Target Range (BWS) (0 - 100)[mm]) >> "); //片側可動域入力
M[2].Target_Range = Double_Stdin(0,100);
printf("\n Input Compliance (horizontal) (0.0 - 20.0)[mm/N]) >> "); //コンプライアンス定数入力
M[1].Compliance = Double_Stdin(0,20);
Input_Timer_Parameter(); //サンプリング間隔と記録時間を入力
M[1].Counter_Past = Count_Read(1); //カウンタ初期値
M[2].Counter_Past = Count_Read(2);
File_Open(); //ファイルオープン
BWS_and_Compliance_h();
////////////////////////////////////////////////////////////////////////
Timer1(Samp_Cycle,Rec_Time,BWS_and_Compliance_drive); //タイマon
////////////////////////////////////////////////////////////////////////
DA_Write(1,2048); //2.5V出力
DA_Write(2,2048);
File_Close(); //ファイルクローズ
sleep(1);
Savexy();
Sensors_Display();
Returnxy(); //カーソル位置復帰
printf("\n Now Check is End. Again? -- Y or N >> ");
yes = Yes_or_No();
if(yes=='n')Flag=0;
}
}
/***************************************************************************************************
*FunctionName：SetGB_LedValue
*Description：设置发光二极管亮度 0 - 1023,实际测试中最大320
*Input：None
*Output：None
*Author：xsx
*Data：2016年4月22日18:19:29
***************************************************************************************************/
void SetGB_LedValue(unsigned short value)
{
unsigned short temp = value;
DA_CS_H();
delay_us(10);
DA_CS_L();
delay_us(50);
GB_LedValue = value;
//低2位为0
temp <<= 2;
/*更新DAC B值*/
temp &= ~((unsigned short)1<<(15));
temp &= ~((unsigned short)1<<(12));
/*快速模式*/
temp &= ~((unsigned short)1<<(14));
/*正常模式*/
temp &= ~((unsigned short)1<<(13));
DA_Write(temp);
delay_us(50);
DA_CS_H();
}
//// ２．DA値のチェック ////////////////////////////////////////////////
void DA_Value_Check(void)
{
int Flag=1;
int yes;
int ch;
double Voltage;
int value;
while(Flag){
Display_Title(); //タイトル表示
printf(" <DA Value Check Menu>\n"); //サブタイトル表示
Sensors_Display();
printf("\n Input Channel (1 or 2) >> "); //チャンネル入力
ch = Int_Stdin(1,2);
printf("\n Input Voltage (0 - 5.0)[V]) >> "); //電圧入力
Voltage = Double_Stdin(0,5);
Savexy(); //カーソル位置取得
value = Voltage / 5.0 * 4095.0; //valueに変換
printf("DA Value is %d \n",value);
DA_Write(ch,value); //出力
sleep(1);
Sensors_Display();
Returnxy(); //カーソル位置復帰
printf("\n Now Check is End. Again? -- Y or N >> ");
yes = Yes_or_No();
if(yes=='n')Flag=0;
}
}
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 Force_Const_2(void)
{
int Flag=1;
int yes;
int ch;
while(Flag){
Display_Title(); //タイトル表示
printf(" <Constant Force Control Test Menu>\n"); //サブタイトル表示
Sensors_Display();
printf("\n Input Target Force (0 - 500)[N]) >> "); //回転数入力
M[1].Target_Force = Double_Stdin(0,500);
M[2].Target_Force = Double_Stdin(0,500);
printf("\n Input Gain (UP) (0 - )[rpm/N]) >> "); //ゲイン入力（のぼり）
M[1].Gain1 = Double_Stdin(0,1000);
M[2].Gain1 = Double_Stdin(0,1000);
printf("\n Input Gain (DOWN) (0 - )[rpm/N]) >> "); //ゲイン入力（くだり）
M[1].Gain2 = Double_Stdin(0,1000);
M[2].Gain2 = Double_Stdin(0,1000);
Input_Timer_Parameter(); //サンプリング間隔と記録時間を入力
M[1].Counter_Past = Count_Read(1); //カウンタ初期値
M[2].Counter_Past = Count_Read(2);
File_Open(); //ファイルオープン
Force_Const_h2();
////////////////////////////////////////////////////////////////////////
Timer2(Samp_Cycle,Rec_Time,Force_Const_Drive2,1); //タイマon
////////////////////////////////////////////////////////////////////////
DA_Write(1,2048); //2.5V出力
DA_Write(2,2048);
File_Close(); //ファイルクローズ
sleep(1);
Savexy();
Sensors_Display();
Returnxy(); //カーソル位置復帰
printf("\n Now Check is End. Again? -- Y or N >> ");
yes = Yes_or_No();
if(yes=='n')Flag=0;
}
}
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;
}
void Step_Drive2(int ch)
{
double RPS;
if(count == 0){
printf("\n Value 1 : %d",M[ch].DA_Value1);
DA_Write(ch,M[ch].DA_Value2);
}
M[ch].Counter = Count_Read(ch);
RPS = (M[ch].Counter-M[ch].Counter_Past)/(2000.0*Reduction_Ratio)/(Samp_Cycle/1000.0);
M[ch].Velocity = RPS * 5.0;
M[ch].RPM = RPS * 60.0;
Step_Log2(ch);
M[ch].Counter_Past = M[ch].Counter;
}
//// ３．速度のチェック ////////////////////////////////////////////////
void Velocity_Check(void)
{
int Flag=1;
int yes;
int ch;
double Voltage;
int value;
while(Flag){
Display_Title(); //タイトル表示
printf(" <Velocity Check Menu>\n"); //サブタイトル表示
Sensors_Display();
printf("\n Input Channel (1 or 2) >> "); //チャンネル入力
ch = Int_Stdin(1,2);
printf("\n Input Voltage (0 - 5.0)[V]) >> "); //電圧入力
Voltage = Double_Stdin(0,5);
Input_Timer_Parameter(); //サンプリング間隔と記録時間を入力
value = Voltage / 5.0 * 4095.0; //valueに変換
M[ch].Counter_Past = Count_Read(ch); //カウンタ初期値
DA_Write(ch,value); //出力
///////////////////////////////////////////////
Timer2(Samp_Cycle,Rec_Time,Velocity_Display,ch); //タイマon
DA_Write(ch,2048); //2.5V出力
sleep(1);
Savexy();
Sensors_Display();
Returnxy(); //カーソル位置復帰
printf("\n Now Check is End. Again? -- Y or N >> ");
yes = Yes_or_No();
if(yes=='n')Flag=0;
}
}
//// ４．速度ステップ入力 ////////////////////////////////////////////////
void Velocity_Step(void)
{
int Flag=1;
int yes;
int ch;
while(Flag){
Display_Title(); //タイトル表示
printf(" <Velocity Step Response Test Menu>\n"); //サブタイトル表示
Sensors_Display();
printf("\n Input Channel (1 or 2) >> "); //チャンネル入力
ch = Int_Stdin(1,2);
printf("\n Input RPM 1 (-830 - 830)[RPM]) >> "); //回転数入力
M[ch].Target_RPM1 = Double_Stdin(-830,830);
M[ch].DA_Value1 = (M[ch].Target_RPM1*0.003+2.5057)/5.0*4095.0; //valueに変換
printf("\n Input RPM 2 (-830 - 830)[RPM]) >> "); //回転数入力
M[ch].Target_RPM2 = Double_Stdin(-830,830);
M[ch].DA_Value2 = (M[ch].Target_RPM2*0.003+2.5057)/5.0*4095.0; //valueに変換
printf("\n Input Sampling Cycle (1-100)[msec] >>");
Samp_Cycle = Int_Stdin(1,100);
M[ch].Counter_Past = Count_Read(ch); //カウンタ初期値
Rec_Time = 2.0;
File_Open(); //ファイルオープン
Step_h(ch);
////////////////////////////////////////////////////////////////////////
Timer2(Samp_Cycle,Rec_Time,Step_Drive1,ch); //タイマon
Timer2(Samp_Cycle,Rec_Time,Step_Drive2,ch); //タイマon
////////////////////////////////////////////////////////////////////////
DA_Write(ch,2048); //2.5V出力
File_Close(); //ファイルクローズ
sleep(1);
Savexy();
Sensors_Display();
Returnxy(); //カーソル位置復帰
printf("\n Now Check is End. Again? -- Y or N >> ");
yes = Yes_or_No();
if(yes=='n')Flag=0;
}
}
///// 制御ボードイニシャライズ関数//////////////////////////////////////////////
void Board_Open(void)
{
int ch;
ThreadCtl(_NTO_TCTL_IO,0);
mmap_device_io(BIO_SIZE,0x300);
///// カウンタ初期化 /////
out8(Count_com,0x13); //A-2-phase x4
out8(Count_com,0x1b); //B-2-phase x4
out8(Count_com,0x14); //A-count enable set
out8(Count_com,0x1c); //B-count enable set
out8(Count_com,0x16); //separate mode
out8(Count_com,0x0e); //level CLR
out8(Count_com,0x0f); //edge CLR
for(ch=1; ch<=Channel; ch++){
M[ch].AD_Init = 2048; //とりあえずのADの初期値
DA_Write(ch,2048); //2.5V出力
}
}
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;
}
