File content as of revision 0:7edbb986d2d7:

/* JKsoft Wallbot BLE Library
 *
 * wallbotble.cpp
 *
 * Copyright (c) 2010-2014 jksoft
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "mbed.h"
#include "wallbotble.h"



wallbotble::wallbotble() :  	
	_right_motor(P0_28,P0_30,P0_0) , 
	_left_motor(P0_29,P0_23,P0_24) ,		
	_sw(P0_16,P0_17),
	_outlow(P0_20),
	_statusled(P0_18,P0_19),
	//_f_sensor1(P0_2),_f_sensor2(P0_3),_f_sensor3(P0_4),_f_sensor4(P0_5),
	_f_sensor1(P0_5),_f_sensor2(P0_4),_f_sensor3(P0_3),_f_sensor4(P0_2),
	_right_enc(P0_8),
	_left_enc(P0_6),
	_ctrl_r(Kp, Ki, Kd, RATE),
	_ctrl_l(Kp, Ki, Kd, RATE)
	{ 
		_right_motor = 0.0;
	    _left_motor = 0.0;
	    _outlow = 0;	
		_statusled = 0x3;
		_sw.mode(PullUp);
		//エンコーダのセットアップ（位相差がないのでA相のみを使う簡易エンコーダ）
    	_right_enc.mode(PullNone);
    	_left_enc.mode(PullNone);

		_right_pulses = 0;
		_left_pulses =0;
		
		_right_timer.start();
		_left_timer.start();
		
	    _right_enc.rise(this, &wallbotble::right_count);
	    _right_enc.fall(this, &wallbotble::right_count);
	    
	    _left_enc.rise(this, &wallbotble::left_count);
	    _left_enc.fall(this, &wallbotble::left_count);
	    
		//ラインセンサキャリブレーション値のリセット
		for(int i = 0 ; i < 4 ; i++)
		{
			_calibratedMinimum[i] = 65535;
			_calibratedMaximum[i] = 0;
		}
		//PIDの初期化
	    _ctrl_r.setInputLimits(0,100);
	    _ctrl_l.setInputLimits(0,100); 
	    _ctrl_r.setOutputLimits(0, 1.0);
    	_ctrl_l.setOutputLimits(0, 1.0);
	
		 _right_back = false;
		 _left_back = false;
	    
	}
void wallbotble::control_enable(bool enable){
	if(enable)
		_control_tic.attach(this, &wallbotble::tick_callback,RATE);
	else
		_control_tic.detach();
		
	_ctrl_r.reset();
	_ctrl_l.reset();
		
}
void wallbotble::tick_callback(){
	//現状態を与える
    _ctrl_r.setProcessValue(get_right_rpm());
    _ctrl_l.setProcessValue(get_left_rpm());

	//制御指令
	if(_right_back)
	    _right_motor = -1*_ctrl_r.compute() + ff_r;
	else
	    _right_motor = _ctrl_r.compute() + ff_r;
	    
	if(_left_back)
		_left_motor = -1*_ctrl_l.compute() + ff_l;
    else
	    _left_motor = _ctrl_l.compute() + ff_l;
    	
}

void wallbotble::SetRPM(float leftRPM, float rightRPM){
	_right_back = false;
	_left_back = false;
	
	ff_r = 0.022*rightRPM;
	ff_l = 0.022*leftRPM;
	
	if(rightRPM < 0){
		_right_back = true;
		rightRPM = -1 *rightRPM; 
	}
	if(leftRPM < 0){
		_left_back = true;
		leftRPM = -1 *leftRPM;
	}
	
    _ctrl_r.setSetPoint(rightRPM);	
	_ctrl_l.setSetPoint(leftRPM);
}

float wallbotble::get_right_rpm(){
	//500ms以上パルスが来ていなければ0
	if(_right_timer.read_ms() > 500)
		_right_rpm = 0;
	
	return _right_rpm;
}

float wallbotble::get_left_rpm(){
	//500ms以上パルスが来ていなければ0
	if(_left_timer.read_ms() > 500)
		_left_rpm = 0;
		
	return _left_rpm;
}

void wallbotble::right_count(){
	_right_pulses++;

	static int _right_times[4];
	
	_right_times[_right_pulses%4] = _right_timer.read_ms();
	
	_right_timer.reset();
	_right_timer.start();

	_right_rpm = (60.0/((float)(_right_times[0]+_right_times[1]+_right_times[2]+_right_times[3])/1000.0)) * 4.0/PulsesPerRev ;

}
void wallbotble::left_count(){
	_left_pulses++;
	
	static int _left_times[4];
	
	_left_times[_left_pulses%4] = _left_timer.read_ms();
	
	_left_timer.reset();
	_left_timer.start();

	_left_rpm = (60/((float)(_left_times[0]+_left_times[1]+_left_times[2]+_left_times[3])/1000))* 4/PulsesPerRev;
}
// 自動キャリブレ
void wallbotble::auto_calibrate()
{
    control_enable(false);
    Timer time;
    time.start();
    left_turn(0.7);  
    while(time.read_ms() < 1000)  
        f_sensor_calibrate();  
    right_turn(0.7);  
    while(time.read_ms() < 3000)  
        f_sensor_calibrate();  
    left_turn(0.7);  
    while(time.read_ms() < 4000)  
        f_sensor_calibrate();   
    stop(); 
    time.stop();
}

// キャリブレーションのリセット
void wallbotble::resetCalibration(){
	for(int i = 0 ; i < 4 ; i++)
	{
		_calibratedMinimum[i] = 65535;
		_calibratedMaximum[i] = 0;
	}
}

void wallbotble::left_motor (float duty) {
    _left_motor = duty;
}

void wallbotble::right_motor (float duty) {
    _right_motor = duty;
}

void wallbotble::forward (float duty) {
    _left_motor = duty;
	_right_motor = duty;
}

void wallbotble::backward (float duty) {
    _left_motor = -duty;
	_right_motor = -duty;
}

void wallbotble::left_turn (float duty) {
    _left_motor = -duty;
	_right_motor = duty;
}

void wallbotble::right_turn (float duty) {
    _left_motor = duty;
	_right_motor = -duty;
}

void wallbotble::stop (void) {
    _right_motor = 0.0;
    _left_motor = 0.0;
}

void wallbotble::set_led(char bit) {
	bit |= bit >> 2;
	_statusled = (bit ^ 0x03) & 0x03;
}

void wallbotble::set_led1(char bit) {
	if( bit )
	{
		bit = _statusled ^ 0x03;
		bit = bit | 0x1;
	}
	else
	{
		bit = _statusled ^ 0x03;
		bit = bit & 0x2;
	}
	set_led(bit);
}

void wallbotble::set_led2(char bit) {
	if( bit )
	{
		bit = _statusled ^ 0x03;
		bit = bit | 0x2;
	}
	else
	{
		bit = _statusled ^ 0x03;
		bit = bit & 0x1;
	}
	set_led(bit);
}

/**センサ値を10回読み込み、最大値と最小値を取得する
* センサ毎にライン→ライン外で2回この関数を呼ぶイメージ
* 
*/

void wallbotble::f_sensor_calibrate(void) {

	unsigned short max_sensor_values[4];
	unsigned short min_sensor_values[4];
	
	for(char j=0;j<10;j++)
	{
		unsigned short sensor_values[4] = { _f_sensor1.read_u16(),
											 _f_sensor2.read_u16(),
											 _f_sensor3.read_u16(),
											 _f_sensor4.read_u16()
										   };
		   
		for(int i=0;i<4;i++)
		{
			// set the max we found THIS time
			if(j == 0 || max_sensor_values[i] < sensor_values[i])
				max_sensor_values[i] = sensor_values[i];

			// set the min we found THIS time
			if(j == 0 || min_sensor_values[i] > sensor_values[i])
				min_sensor_values[i] = sensor_values[i];
		}
	}
	
	// record the min and max calibration values
	for(char i=0;i<4;i++)
	{
		if(min_sensor_values[i] > _calibratedMaximum[i])	//より大きな最小値へ更新
			_calibratedMaximum[i] = min_sensor_values[i];
		if(max_sensor_values[i] < _calibratedMinimum[i])	//より小さな最大値へ更新
			_calibratedMinimum[i] = max_sensor_values[i];
	}
	
}

// ラインの推定位置を返す。-1500～0（中央）～1500
// ラインセンサの出力が低すぎる場合（ロスト）は、前回値から判定し、0か3000を返す
short wallbotble::GetLinePosition(void) {
	unsigned char i, on_line = 0;
	unsigned long avg = 0; // this is for the weighted total, which is long before division
	unsigned int sum = 0; // this is for the denominator which is <= 64000
	static int last_value=0; // assume initially that the line is left.
	
	readCalibrated();
	
	for(i=0;i<4;i++) {
		int value = sensor_values[i];

		// keep track of whether we see the line at all
		if(value > 450) {
			on_line = 1;
		}
		
		// only average in values that are above a noise threshold
		if(value > 50) {
			avg += (long)(value) * (i * 1000);
			sum += value;
		}
	}

	if(!on_line)//ラインをロストしていたら、前回値から右端、左端として出力する
	{
		if(last_value < 0)
			return -1500;
		else
			return 1500;

	}

	last_value = avg/sum -1500;

	return last_value;
}

// ラインセンサ値を補正し0から1000にマッピングしてsensor_valuesにロードする。
// 0 はキャリブレートmin以下を、1000はキャリブレートmax以上を意味します。
// キャリブレートmin,maxはセンサ毎にストアされています。
void wallbotble::readCalibrated(){
	unsigned short raw_values[4] = { _f_sensor1.read_u16(),
									 _f_sensor2.read_u16(),
									 _f_sensor3.read_u16(),
									 _f_sensor4.read_u16()
								   };

#if DBG
	printf("raw(%d,%d,%d,%d)",raw_values[0],raw_values[1],raw_values[2],raw_values[3]);
#endif
		   
   	for(int i=0;i<4;i++)
	{
   		unsigned int denominator = _calibratedMaximum[i] - _calibratedMinimum[i];
		
		signed int x = 0;
		if(denominator != 0)
			x = (((signed long)raw_values[i]) - _calibratedMinimum[i]) * 1000 / denominator;
		
		if(x < 0)
			x = 0;
		else if(x > 1000)
			x = 1000;
			
		sensor_values[i] = x;
	}
   
}

int wallbotble::GetSw(void) {
	return(_sw ^ 0x3);
}