C++
1#include "stdafx.h" 2#include "contec.h" 3#include <iostream> 4#define Kv -0.002 //速度ゲインを入力(P) 5#define Ki -0.01 //角度ゲインを入力(I) 6 7 double theta = 0.0; //角度の初期値 8 double theta_d = 0.0; //角速度の初期値 9 double theta_r = 0.0; //目標角度の初期値 10 double theta_dr = 0.0; //目標角速度の初期値 11 double theta_plus; //1ステップ先の角度 12 double theta_dr_plus; //1ステップ先の目標角速度 13 double delta_theta; //角度差 14 double delta_theta_r; //目標角度の変化分 15 long a[4]; //カウント値を代入できるチャネル数は4つ(今回は1つ) 16 int N = 1; //読み取るごとにカウント 17 double timer = 10.0; //タイマの設定[ms] 18 cont c; 19 20 21 std::ofstream ofs("otn-experiment.csv"); 22 23void CallBackProc() //タイマーで呼び出す関数 24 { 25 if(((N*timer)/1000.0) <= 2.0) 26 { 27 theta_dr_plus = 0; 28 std::cout << "time1" << std::endl; 29 }else if(2.0<((N*timer)/1000.0) && ((N*timer)/1000.0) <= 4.0) //目標角速度[rad/s]軌道の式 30 { 31 theta_dr_plus = 15.0*2.0*3.14*((100.0*(((N*timer)/1000.0)-2.0))/128.0); 32 std::cout << "time2" << std::endl; 33 }else if(4.0<((N*timer)/1000.0) && ((N*timer)/1000.0) <= 10.0) 34 { 35 theta_dr_plus = 15.0*2.0*3.14*(200.0/128.0); 36 std::cout << "time3" << std::endl; 37 }else if(10.0 < ((N*timer)/1000.0) && ((N*timer)/1000.0) <= 12.0) 38 { 39 theta_dr_plus = (15.0*2.0*3.14*(200.0/128.0))-(15.0*2.0*3.14*((100.0*(((N*timer)/1000.0)-10.0))/128.0)); 40 std::cout << "time4" << std::endl; 41 } 42 43 c.ContRead(a); //カウント値読み込み 44 45 delta_theta_r = (theta_dr+theta_dr_plus)*(timer/1000)*0.5; //台形法により、目標角度の変化分を算出 46 theta_r = theta_r + delta_theta_r; //目標角度を算出 47 48 theta_plus = 2.0*3.14*((a[0]-8388607.0)/(2.0*4.0*128.0)); //現在の回転角度[rad]を算出 49 delta_theta = theta_plus - theta; //角度差 50 theta_d = delta_theta/(timer/1000.0); //角速度算出[rad/s] 51 52 double V; 53 54 if(theta_dr_plus == 0) 55 { 56 V = 1.1; 57 }else if(theta_dr_plus =! 0) 58 { 59 V = Kv*(theta_dr-theta_d)+Ki*(theta_r-theta_plus); //出力電圧 60 } 61 62 63 64 if(V>10.0) //出力電圧のフィルタ 65 { 66 V = 10.0; 67 }else if(V<-10.0) 68 { 69 V = -10.0; 70 } 71 72 c.OutVol(V); 73 74 theta = theta_plus; 75 theta_dr = theta_dr_plus; 76 77 78 ofs<<a[0]<<","<<a[1]<<","<<a[2]<<","<<a[3]<<","<<V<<","<<((N*timer)/1000)<<","<<theta_dr<<","<<theta_d<<std::endl; 79 80 N++; 81 82 } 83 84int _tmain(int argc, _TCHAR* argv[]) 85{ 86 long winapi; 87 c.ContClear(); 88 std::cout << "Clear()" << std::endl; 89 c.ContStart(); 90 std::cout << "Start()" << std::endl; 91 92 93 int an; 94 std::cin >> an; 95 96 winapi=CntTimerCallbackProc(mpara::idCnt , CallBackProc , NULL); //タイマー準備 97 98 99 while(1) 100 { 101 winapi=CntNotifyTimer(mpara::idCnt , timer , NULL); //設定した時間ごとにタイマーの呼び出し 102 103 if((N*timer/1000.0) > 12.0) //目標軌道が終了したら、ループから抜ける 104 { 105 theta_dr_plus = 0.0; 106 break; 107 std::cout << "fin" << std::endl; 108 } 109 } 110 std::cout << "out" << std::endl; 111 c.ContEnd(); 112 113 int b; 114 std::cin >> b ; 115 return 0; 116} 117
if(theta_dr_plus == 0)を入れてから、目標角速度のグラフは0付近のままになっています。外すと、正しいグラフが出ます。正しいグラフは2秒後に台形速度軌道のグラフが出るというものです。
theta_dr_plus = 0.0のときだけ、例外処理として出力電圧を1.1Vにしたいので、このようなif文を入れました。
theta_dr_plusが0でないときは、目標速度軌道に追従させたいと思ってます。
どなたか、原因を教えてくれませんか?
回答2件
あなたの回答
tips
プレビュー
バッドをするには、ログインかつ
こちらの条件を満たす必要があります。
2016/11/02 13:56
2016/11/02 18:02
2016/11/02 18:22