Revision 6:9fd87d75a24b, committed 2018-05-17

Comitter:

c201075

Date:

Thu May 17 02:30:58 2018 +0000

Parent:

5:eeabd90b6d62

Child:

7:5aa479fe5d0b

Commit message:

??????????????RPM????????????????

Changed in this revision

--- a/main.cpp	Thu May 17 01:41:56 2018 +0000
+++ b/main.cpp	Thu May 17 02:30:58 2018 +0000
@@ -2,183 +2,20 @@
 #include "MPU6050.h"
 #include "BLEDevice.h"
 #include "wallbotble.h"
-#include "RCBController.h"
-#include "Adafruit_LEDBackpack.h"
-#include "Adafruit_GFX.h"
-#include "pictLIB.h"
+//#include "RCBController.h"
+//#include "Adafruit_LEDBackpack.h"
+//#include "Adafruit_GFX.h"
+//#include "pictLIB.h"
 
 
 #define DEBUG
 
-enum _mode {
-    Nomal = 0,
-    LineFollow,
-    Challenge,
-    CaaS
-};
-
 Serial pc(USBTX, USBRX);
 BLEDevice  ble;
-Ticker ticker;
 MPU6050 mpu;
 wallbotble wb;
 
 myI2C i2c(P0_22,P0_21);
-//Adafruit_8x8mat96rix matrix1 = Adafruit_8x8matrix(&i2c);
-//Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c);
-//PICTLIB pl(16,16);
-
-int mode = Nomal;
-
-char bValue = 0;
-
-
-/* RCBController Service */
-static const uint16_t RCBController_service_uuid = 0xFFF0;
-static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
-static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
-uint8_t RCBControllerPayload[10] = {0,};
-
-GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
-                                    GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE |
-                                    GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
-//static uint8_t _bValue = 0x00;
-static uint8_t _mValue[10] = {0,};
-GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
-                          GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
-
-GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
-GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
-
-RCBController controller;
-
-
-void SendMessage(char *msg)
-{
-    if (!ble.getGapState().connected) {
-        return;
-    }
-    int len = strlen(msg);
-
-    if(len < 10) {
-        strcpy( (char*)_mValue ,msg );
-        ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
-    }
-}
-
-void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params)
-{
-    wb.set_led2(1);
-#ifdef DEBUG
-    pc.printf("Connected\n\r");
-#endif
-}
-
-void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
-{
-    wb.stop();
-
-    ble.startAdvertising();
-    wb.set_led2(0);
-#ifdef DEBUG
-    pc.printf("Disconnected\n\r");
-#endif
-}
-
-void periodicCallback(void)
-{
-    if (!ble.getGapState().connected) {
-        return;
-    }
-    if( (bValue == 0)&&(wb.GetLinePosition() != 0) ) {
-        // Game over
-        wb.stop();
-        bValue = 10;
-    }
-    if( bValue > 0 ) {
-        wb.stop();
-        SendMessage("GAME OVER");
-        bValue--;
-        if( bValue == 0 ) {
-            wb.set_led1(0);
-            SendMessage(" NOMAL  ");
-            mode = Nomal;
-            ticker.detach();
-        }
-    }
-}
-
-
-// GattEvent
-void onDataWritten(const GattCharacteristicWriteCBParams *params)
-{
-    if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow )) {
-        float right_factor;
-        float left_factor;
-
-        memcpy( &controller.data[0], params->data , params->len );
-#ifdef DEBUG
-        pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
-                  controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
-#endif
-
-        if(mode == Challenge) {
-            float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
-
-            float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
-            float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
-
-            if( /*controller.status.B ==*/ 1 ) {
-                wb.left_motor(left_factor);
-                wb.right_motor(right_factor);
-            } else if( controller.status.A == 1 ) {
-                wb.left_motor(-right_factor);
-                wb.right_motor(-left_factor);
-            } else {
-                wb.forward(1.0);
-            }
-        } else {
-            if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) ) {
-                left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
-                right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
-
-                wb.left_motor(left_factor);
-                wb.right_motor(right_factor);
-            } else {
-                float factor = ((float)((int)controller.status.AcceleX -128) / 128.0);
-
-                right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
-                left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
-
-                if( controller.status.B == 1 ) {
-                    wb.left_motor(left_factor);
-                    wb.right_motor(right_factor);
-                } else if( controller.status.A == 1 ) {
-                    wb.left_motor(-right_factor);
-                    wb.right_motor(-left_factor);
-                } else if( controller.status.UP == 1 ) {
-                    wb.forward(1.0);
-                } else if( controller.status.DOWN == 1 ) {
-                    wb.backward(1.0);
-                } else if( controller.status.RIGHT == 1 ) {
-                    wb.right_turn(1.0);
-                } else if( controller.status.LEFT == 1 ) {
-                    wb.left_turn(1.0);
-                } else {
-                    wb.stop();
-                }
-                if((controller.status.UP == 1)&&(controller.status.DOWN == 1)) { // START BUTTON
-                    SendMessage("Challenge");
-                    wb.set_led1(1);
-                    mode = Challenge;
-                    wb.stop();
-                    ticker.attach(periodicCallback, 0.1);
-                }
-            }
-        }
-    }
-}
-
 
 /**************************************************************************/
 /*!
@@ -192,49 +29,45 @@
     int16_t gx, gy, gz;
 
     //mpu.initialize();
-    
-    //wb.auto_calibrate();
 
-#ifdef DEBUG
     pc.baud(115200);
     pc.printf("Start\n\r");
-#endif
 
-    ble.init();
 
+    //ble.init();
     //イベント時のコールバック関数
-    ble.onConnection(onConnected);
-    ble.onDisconnection(onDisconnected);
-    ble.onDataWritten(onDataWritten);
+    //ble.onConnection(onConnected);
+    //ble.onDisconnection(onDisconnected);
+    //ble.onDataWritten(onDataWritten);
 
     /* setup advertising */
     //クラシックBTはサポートせず、BLEデバイスとして認識してもらう
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    //ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
     //デバイスがセントラルに接続可能であることを設定
-    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    //ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
     //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
-                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
+    //ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
     //16Bit短縮UUIDの設定
-    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
-                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
+    //ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,(const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
 	//Advertiseパケットの送信周期
-    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
+    //ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
 
 	//サービスの登録
-    ble.addService(RCBControllerService);
+    //ble.addService(RCBControllerService);
 
     //Advertizeの開始
-    ble.startAdvertising();
+    //ble.startAdvertising();
 
 	//wb.move(30,30);
 	pc.printf("auto calibrate start\n\r");
     wb.auto_calibrate();
+#ifdef DEBUG
     for(int i = 0 ; i < 4 ; i++)
 	{
 		pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]);
 	}
 	pc.printf("\n\rauto calibrate end\n\r");
+#endif
     
 #if 0 //エンコーダキャリブレーション用コード
 	BusIn enc(P0_8,P0_10,P0_6,P0_7);
@@ -249,9 +82,9 @@
     while (true) {
 		//mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 		#ifdef DEBUG
-			pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,wb.get_left_rpm(),wb.get_right_rpm());
+			pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,wb.left_rpm,wb.right_rpm);
 			pc.printf("LinePos:%d ",wb.GetLinePosition());
-			pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
+			//pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
 		    //pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)\n\r",ax,ay,az,gx,gy,gz);
 		    pc.printf("\n\r");
 		    wait_ms(10);

--- a/wallbotble/wallbotble/TB6612/TB6612.cpp	Thu May 17 01:41:56 2018 +0000
+++ b/wallbotble/wallbotble/TB6612/TB6612.cpp	Thu May 17 02:30:58 2018 +0000
@@ -26,6 +26,7 @@
 //  arg
 //   float speed -1.0 - 0.0 - 1.0
 void TB6612::speed(float speed) {
+    nowspeed = speed;
     
     if( timer_flag == true )    return;
     

--- a/wallbotble/wallbotble/TB6612/TB6612.h	Thu May 17 01:41:56 2018 +0000
+++ b/wallbotble/wallbotble/TB6612/TB6612.h	Thu May 17 02:30:58 2018 +0000
@@ -20,6 +20,7 @@
     {
         speed(value);
     }
+    float nowspeed;
     
 protected:
     PwmOut _pwm;

--- a/wallbotble/wallbotble/wallbotble.cpp	Thu May 17 01:41:56 2018 +0000
+++ b/wallbotble/wallbotble/wallbotble.cpp	Thu May 17 02:30:58 2018 +0000
@@ -34,7 +34,8 @@
 	_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_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),
@@ -52,81 +53,92 @@
 		_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,300); //仮
-		_ctrl_l.setInputLimits(0,300); //仮
-	    
-	    _ctrl_r.setOutputLimits(0, 1.0);
-	    _ctrl_l.setOutputLimits(0, 1.0);
+	    _ctrl_r.setInputLimits(0,100);
+	    _ctrl_l.setInputLimits(0,100); 
 	    
-		// RATE(sec)毎にrpm更新と制御を行う。
-		#ifndef DEBUG
-		_control_tic.attach(this, &wallbotble::tick_callback,RATE);
-		#endif
-		
+	    _ctrl_r.setOutputLimits(-1.0, 1.0);
+	    _ctrl_l.setOutputLimits(-1.0, 1.0);
+	    
 	}
-
+void wallbotble::contro_enable(bool enable){
+	if(enable)
+		_control_tic.attach(this, &wallbotble::tick_callback,RATE);
+	else
+		_control_tic.detach();
+}
 void wallbotble::tick_callback(){
-	
-	_right_rpm = (60/RATE) * (float)_right_pulses/PulsesPerRev;
-	_left_rpm = (60/RATE) * (float)_left_pulses/PulsesPerRev;
-	
-	_right_pulses = 0;
-	_left_pulses =0;
-	
 	//現状態を与える
-    _ctrl_r.setProcessValue(_right_rpm);
-    _ctrl_l.setProcessValue(_left_rpm);
+    _ctrl_r.setProcessValue(right_rpm);
+    _ctrl_l.setProcessValue(left_rpm);
 
 	//制御指令
     //right_motor = _ctrl_r.compute();
     //left_motor = _ctrl_l.compute();
 }
 
-void wallbotble::move(float leftRPM, float rightRPM){
+void wallbotble::SetRPM(float leftRPM, float rightRPM){
 	_ctrl_l.setSetPoint(leftRPM);
     _ctrl_r.setSetPoint(rightRPM);	
 }
 
-
-float wallbotble::get_right_rpm(){
-	return _right_rpm;
-}
-float wallbotble::get_left_rpm(){
-	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 ;
+
+	if(_right_motor.nowspeed < 0)
+		right_rpm = -1 * right_rpm;
 }
 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;
+	if(_left_motor.nowspeed < 0)
+		left_rpm = -1 * left_rpm;
 }
 // 自動キャリブレ
 void wallbotble::auto_calibrate()
 {
     Timer time;
     time.start();
-    left_turn(0.5);  
-    while(time.read_ms() < 250)  
+    left_turn(0.7);  
+    while(time.read_ms() < 1000)  
         f_sensor_calibrate();  
-    right_turn(0.5);  
-    while(time.read_ms() < 750)  
+    right_turn(0.7);  
+    while(time.read_ms() < 3000)  
         f_sensor_calibrate();  
-    left_turn(0.5);  
-    while(time.read_ms() < 1000)  
+    left_turn(0.7);  
+    while(time.read_ms() < 4000)  
         f_sensor_calibrate();   
     stop(); 
     time.stop();
@@ -250,7 +262,7 @@
 
 // ラインの推定位置を返す。0～1500（中央）～3000
 // ラインセンサの出力が低すぎる場合（ロスト）は、前回値から判定し、0か3000を返す
-unsigned short wallbotble::GetLinePosition(void) {
+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
@@ -275,14 +287,14 @@
 
 	if(!on_line)//ラインをロストしていたら、前回値から右端、左端として出力する
 	{
-		if(last_value < 1500)
-			return 0;
+		if(last_value < 0)
+			return -1500;
 		else
-			return 3000;
+			return 1500;
 
 	}
 
-	last_value = avg/sum;
+	last_value = avg/sum -1500;
 
 	return last_value;
 }

--- a/wallbotble/wallbotble/wallbotble.h	Thu May 17 01:41:56 2018 +0000
+++ b/wallbotble/wallbotble/wallbotble.h	Thu May 17 02:30:58 2018 +0000
@@ -130,10 +130,10 @@
      */
     void stop (void);
 	
-    /** ラインの推定値を返す。 左端0～中央1500～右端3000
+    /** ラインの推定値を返す。 左端-1500～中央0～右端1500
      * 一定以上の検出がない場合は前回値から右端、左端と判定
      */	
-	unsigned short GetLinePosition(void);
+	short GetLinePosition(void);
 	
     /** Get switch .(switch OFF:0 or ON:1 return.)
      *
@@ -155,8 +155,8 @@
      */	
 	void set_led2(char bit);
 	
-	// RPM指令で左右モータを制御する。バックができない。
-	void move(float leftRPM, float rightRPM);    
+	// RPM指令で左右モータを制御する。
+	void SetRPM(float leftRPM, float rightRPM);    
 		
 	// キャリブレーションのリセット
 	void resetCalibration();
@@ -170,23 +170,32 @@
 	unsigned short sensor_values[4];  
 	
 	// 左右のモーター回転数を返す
-	float get_right_rpm();
-	float get_left_rpm();
+	float right_rpm;
+	float left_rpm;
+	
+	void contro_enable(bool enable);	
+
+	unsigned short _calibratedMinimum[4];  
+	unsigned short _calibratedMaximum[4];  
 	
 	
-
+    unsigned short _right_pulses;
+    unsigned short _left_pulses;
+    
 	private :
 
+	Timer _right_timer;
+	Timer _left_timer;
 	void right_count();
 	void left_count();
 	
 	void tick_callback();
 	
 	Ticker _control_tic;
-	float _right_rpm;
-	float _left_rpm;
+
 	TB6612 _right_motor;
 	TB6612 _left_motor;
+	
 	BusIn _sw;
 	DigitalOut _outlow;
 	BusOut _statusled;
@@ -195,14 +204,10 @@
 	AnalogIn _f_sensor3;
 	AnalogIn _f_sensor4;
 	int _sensor_gain;
-	unsigned short _calibratedMinimum[4];  
-	unsigned short _calibratedMaximum[4];  
 	
     InterruptIn _right_enc;
     InterruptIn _left_enc;
     
-    unsigned int _right_pulses;
-    unsigned int _left_pulses;
     
 	PID _ctrl_r;
 	PID _ctrl_l;