Revision 0:7edbb986d2d7, committed 2018-06-04

Comitter:

c201075

Date:

Mon Jun 04 08:32:43 2018 +0000

Commit message:

library

Changed in this revision

diff -r 000000000000 -r 7edbb986d2d7 wallbotble/PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotble/PID.lib	Mon Jun 04 08:32:43 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19

diff -r 000000000000 -r 7edbb986d2d7 wallbotble/TB6612/TB6612.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotble/TB6612/TB6612.cpp	Mon Jun 04 08:32:43 2018 +0000
@@ -0,0 +1,72 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the TOSHIBA. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+
+
+#include "TB6612.h"
+
+// TB6612 Class Constructor
+TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0.0;
+    _pwm.period(0.001);
+    
+    bspeed = 0.0;
+    timer_flag = false;
+}
+
+// Speed Control
+//  arg
+//   float speed -1.0 - 0.0 - 1.0
+void TB6612::speed(float speed) {
+    nowspeed = speed;
+    
+    if( timer_flag == true )    return;
+    
+    bspeed = speed;
+    
+    if( speed > 0.0 )
+    {
+        _pwm = speed;
+        _fwd = 1;
+        _rev = 0;
+    }
+    else if( speed < 0.0 )
+    {
+        _pwm = -speed;
+        _fwd = 0;
+        _rev = 1;
+    }
+    else
+    {
+        _fwd = 1;
+        _rev = 1;
+    }
+}
+
+
+// Speed Control with time-out
+//  arg
+//   float sspeed:-1.0 - 0.0 - 1.0
+//   float time  :0.0-
+void TB6612::move(float sspeed , float time)
+{
+    speed(sspeed);
+    timer_flag = true;
+    timer.attach(this,&TB6612::timeout,time);
+}
+
+
+void TB6612::timeout()
+{
+    timer_flag = false;
+    speed(bspeed);
+}
+

diff -r 000000000000 -r 7edbb986d2d7 wallbotble/TB6612/TB6612.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotble/TB6612/TB6612.h	Mon Jun 04 08:32:43 2018 +0000
@@ -0,0 +1,36 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the rohm. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+
+#ifndef MBED_TB6612_H
+#define MBED_TB6612_H
+
+#include "mbed.h"
+
+class TB6612 {
+public:
+    TB6612(PinName pwm, PinName fwd, PinName rev);
+    void speed(float speed);
+    void move(float speed , float time);
+    void operator= ( float value )
+    {
+        speed(value);
+    }
+    float nowspeed;
+    
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+    Timeout timer;
+    float    bspeed;
+    bool     timer_flag;
+    void timeout();
+
+};
+
+#endif

diff -r 000000000000 -r 7edbb986d2d7 wallbotble/wallbotble.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotble/wallbotble.cpp	Mon Jun 04 08:32:43 2018 +0000
@@ -0,0 +1,380 @@
+/* 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);
+}
+
+

diff -r 000000000000 -r 7edbb986d2d7 wallbotble/wallbotble.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wallbotble/wallbotble.h	Mon Jun 04 08:32:43 2018 +0000
@@ -0,0 +1,225 @@
+/* JKsoft Wallbot BLE Library
+ *
+ * wallbotble.h
+ *
+ * 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.
+ */
+
+#ifndef WALLBOT_BLE_H
+#define WALLBOT_BLE_H
+
+#include "mbed.h"
+#include "TB6612.h"
+//#include "QEI.h"
+#include "PID.h"
+
+#define PulsesPerRev	24
+
+#define Kp  0.0001
+#define Ki  0.001
+#define Kd  0.0
+#define RATE 0.01 // 制御周期(sec) 0.01=10ms
+
+/** wallbot ble control class
+ *
+ * Example:
+ * @code
+ * // Drive the wwallbot forward, turn left, back, turn right, at half speed for half a second
+
+#include "mbed.h"
+#include "wallbotble.h"
+
+wallbotble wb;
+ 
+int main() {
+
+    wb.sensor_calibrate();
+
+    while(!wb.GetSw())
+    {
+        wb.set_led(wb.GetLinePosition());
+    }
+    
+    wb.forward(1.0);
+    wait (1.0);
+    wb.left(1.0);
+    wait (1.0);
+    wb.backward(1.0);
+    wait (1.0);
+    wb.right(1.0);
+    wait (1.0);
+    
+    wb.stop();
+
+    while(1);
+
+ }
+
+ * @endcode
+ */
+class wallbotble  {
+
+    // Public functions
+public:
+
+    /** Create the wallbot object connected to the default pins
+     */
+    wallbotble();
+    
+    
+    /** Sensor calibrate
+     *
+     */
+    void f_sensor_calibrate (void);
+
+    /** Directly control the speed and direction of the left motor
+     *
+     * @param speed A normalised number -1.0 - 1.0 represents the full range.
+     */
+    void left_motor (float duty);
+
+    /** Directly control the speed and direction of the right motor
+     *
+     * @param speed A normalised number -1.0 - 1.0 represents the full range.
+     */
+    void right_motor (float duty);
+
+    /** Drive both motors forward as the same speed
+     *
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void forward (float duty);
+
+    /** Drive both motors backward as the same speed
+     *
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void backward (float duty);
+
+    /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot
+     *
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void left_turn (float duty);
+
+    /** Drive left motor forward and right motor backwards at the same speed to turn on the spot
+     * @param speed A normalised number 0 - 1.0 represents the full range.
+     */
+    void right_turn (float duty);
+
+    /** Stop both motors
+     *
+     */
+    void stop (void);
+	
+    /** ラインの推定値を返す。 左端-1500～中央0～右端1500
+     * 一定以上の検出がない場合は前回値から右端、左端と判定
+     */	
+	short GetLinePosition(void);
+	
+    /** Get switch .(switch OFF:0 or ON:1 return.)
+     *
+     */	
+	int GetSw(void);
+	
+    /** Set status led .
+     * @param led (bit0:LEFT bit2:RIGHT)
+     */	
+	void set_led(char bit);
+
+    /** Set led1 .
+     * @param led (0:off,1:on)
+     */	
+	void set_led1(char bit);
+
+    /** Set led2 .
+     * @param led (0:off,1:on)
+     */	
+	void set_led2(char bit);
+	
+	// RPM指令で左右モータを制御する。
+	void SetRPM(float leftRPM, float rightRPM);    
+		
+	// キャリブレーションのリセット
+	void resetCalibration();
+
+	// 自動キャリブレーション
+	void auto_calibrate();
+	
+	//ラインセンサ値を補正し0から1000にマッピングしてsensor_valuesにロードする。
+	void readCalibrated();
+	
+	unsigned short sensor_values[4];  
+	
+	
+	void control_enable(bool enable);	
+
+	unsigned short _calibratedMinimum[4];  
+	unsigned short _calibratedMaximum[4];  
+	
+	// 左右のモーター回転数を返す
+	float get_right_rpm();
+	float get_left_rpm();
+	
+    unsigned short _right_pulses;
+    unsigned short _left_pulses;
+    
+private :
+	//ff tearm
+	float ff_r,ff_l;
+	//後退用フラグ
+	bool _right_back;
+	bool _left_back;
+	
+	float _right_rpm;
+	float _left_rpm;
+	
+
+	Timer _right_timer;
+	Timer _left_timer;
+	void right_count();
+	void left_count();
+	
+	void tick_callback();
+	
+	Ticker _control_tic;
+
+	TB6612 _right_motor;
+	TB6612 _left_motor;
+	
+	BusIn _sw;
+	DigitalOut _outlow;
+	BusOut _statusled;
+	AnalogIn _f_sensor1;
+	AnalogIn _f_sensor2;
+	AnalogIn _f_sensor3;
+	AnalogIn _f_sensor4;
+	int _sensor_gain;
+	
+    InterruptIn _right_enc;
+    InterruptIn _left_enc;
+    
+    
+	PID _ctrl_r;
+	PID _ctrl_l;
+};
+
+#endif
\ No newline at end of file