制御プログラムです。なぜか、目標角速度(theta_dr_plus)が0になります。それがわかりません。

Question

#include "stdafx.h"
#include "contec.h"
#include <iostream>
#define Kv   -0.002  //速度ゲインを入力(P)
#define Ki   -0.01   //角度ゲインを入力(I)

    double theta = 0.0;  //角度の初期値
	double theta_d = 0.0;  //角速度の初期値
	double theta_r = 0.0;  //目標角度の初期値
	double theta_dr = 0.0; //目標角速度の初期値
	double theta_plus;  //1ステップ先の角度
	double theta_dr_plus;  //1ステップ先の目標角速度
	double delta_theta; //角度差
	double delta_theta_r;  //目標角度の変化分
	long a[4];   //カウント値を代入できるチャネル数は4つ（今回は1つ）
	int N = 1; //読み取るごとにカウント
	double timer = 10.0;  //タイマの設定[ms]
	cont c;
	

	std::ofstream ofs("otn-experiment.csv");

void CallBackProc()    //タイマーで呼び出す関数
	{
		if(((N*timer)/1000.0) <= 2.0)
		{
			theta_dr_plus = 0;
			std::cout << "time1" << std::endl;
		}else if(2.0<((N*timer)/1000.0) && ((N*timer)/1000.0) <= 4.0)   //目標角速度[rad/s]軌道の式
		{
			theta_dr_plus = 15.0*2.0*3.14*((100.0*(((N*timer)/1000.0)-2.0))/128.0); 
			std::cout << "time2" << std::endl;
		}else if(4.0<((N*timer)/1000.0) && ((N*timer)/1000.0) <= 10.0)
		{
			theta_dr_plus = 15.0*2.0*3.14*(200.0/128.0);
			std::cout << "time3" << std::endl;
		}else if(10.0 < ((N*timer)/1000.0) && ((N*timer)/1000.0) <= 12.0)
		{
			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));
			std::cout << "time4" << std::endl;
		}
		
		c.ContRead(a);  //カウント値読み込み

		delta_theta_r = (theta_dr+theta_dr_plus)*(timer/1000)*0.5;  //台形法により、目標角度の変化分を算出
		theta_r = theta_r + delta_theta_r;  //目標角度を算出
		
		theta_plus = 2.0*3.14*((a[0]-8388607.0)/(2.0*4.0*128.0));  //現在の回転角度[rad]を算出
		delta_theta = theta_plus - theta;  //角度差
        theta_d = delta_theta/(timer/1000.0);     //角速度算出[rad/s]
		
		double V;

		if(theta_dr_plus == 0)
		{
			V = 1.1;
		}else if(theta_dr_plus =! 0)
		{
			V = Kv*(theta_dr-theta_d)+Ki*(theta_r-theta_plus); //出力電圧
		}
		
		

		if(V>10.0) //出力電圧のフィルタ                                                 
		{
			V = 10.0;
		}else if(V<-10.0)
		{
			V = -10.0;
		}

        c.OutVol(V);

		theta = theta_plus;
		theta_dr = theta_dr_plus;


		ofs<<a[0]<<","<<a[1]<<","<<a[2]<<","<<a[3]<<","<<V<<","<<((N*timer)/1000)<<","<<theta_dr<<","<<theta_d<<std::endl;

		N++;

	}

int _tmain(int argc, _TCHAR* argv[])
{
	long winapi;
	c.ContClear();
	std::cout << "Clear()" << std::endl;
	c.ContStart();
	std::cout << "Start()" << std::endl;


	int an;
	std::cin >> an;

	winapi=CntTimerCallbackProc(mpara::idCnt , CallBackProc , NULL); //タイマー準備


	while(1)
	{   
		winapi=CntNotifyTimer(mpara::idCnt , timer , NULL);  //設定した時間ごとにタイマーの呼び出し

		if((N*timer/1000.0) > 12.0)  //目標軌道が終了したら、ループから抜ける
		{
			theta_dr_plus = 0.0;
			break;
			std::cout << "fin" << std::endl;
		}
	}
	std::cout << "out" << std::endl;
	c.ContEnd();
	
	int b;
	std::cin >> b ;
	return 0;
}

if(theta_dr_plus == 0)を入れてから、目標角速度のグラフは0付近のままになっています。外すと、正しいグラフが出ます。正しいグラフは2秒後に台形速度軌道のグラフが出るというものです。
theta_dr_plus = 0.0のときだけ、例外処理として出力電圧を1.1Ｖにしたいので、このようなif文を入れました。
theta_dr_plusが0でないときは、目標速度軌道に追従させたいと思ってます。
どなたか、原因を教えてくれませんか？

Answer 1

まず

if(theta_dr_plus == 0)
        {
            V = 1.1;
        }else if(theta_dr_plus == 0)
        {
            V = Kv*(theta_dr-theta_d)+Ki*(theta_r-theta_plus); //出力電圧
        }

ここの条件が変？？
続いて、ループからのbreakの前に、

while(1)
    {   
        winapi=CntNotifyTimer(mpara::idCnt , timer , NULL);  //設定した時間ごとにタイマーの呼び出し

        if((N*timer/1000.0) > 12.0)  //目標軌道が終了したら、ループから抜ける
        {
            theta_dr_plus = 0.0;
            break;
            std::cout << "fin" << std::endl;
        }
    }

で必ず0.0が入りますね。

Comments

[MasahikoHirata]

まず条件が変と言ったのは、
theta_dr_plus == 0
のelseにまたtheta_dr_plus == 0？
絶対通らないが。。。

[keisuke1995]

すいません。ミスですね。
else if(theta_dr_plus =! 0)です。
theta_dr_plus = 0.0;の文は、消去したいと思います。

[keisuke1995]

あれ？でも、12秒経ってからif文の中のプログラムが実行されるので、12秒以内のときはtheta_dr_plus = 0.0ではないのでは？

Answer 2

前回答者の指摘で解決しているかもしれませんが、
theta_dr_plusはdouble型なので0と比較するのは止めましょう。思惑と違う評価になることがあります
0だと思っても、0とは限りません(そもそも、浮動小数点値と等価比較は止める)
小数点が出ない桁までint型に変換して使いましょう。その際に切り捨てるのか四捨五入するのかは仕様次第。
(例えば、小数点第1位まででよければ、10倍してint型にして比較)

あとつまらないことですが、double型の変数に値詰め込むときには小数点数値で。
(ものによると思いますがdebug目的でoutputすると正しい数値にならないことがある)

if(((N*timer)/1000.0) <= 2.0)
 {
            theta_dr_plus = 0;
 

こことか

Comments

[keisuke1995]

では、theta_dr_plus == 0.0ならよいのでしょうか？
わかりました。
数値すべてに小数点が付くようにします