Revision 11:8142dc8348f4, committed 2018-05-17

Comitter:

c201075

Date:

Thu May 17 06:11:48 2018 +0000

Parent:

10:219a5429130a

Child:

12:96fbb4cb7c48

Commit message:

?????

Changed in this revision

--- a/main.cpp	Thu May 17 05:12:34 2018 +0000
+++ b/main.cpp	Thu May 17 06:11:48 2018 +0000
@@ -74,15 +74,15 @@
 //======================================================================
 void onDataWritten(const GattCharacteristicWriteCBParams *params)
 {   
-    float right_speed;
-    float left_speed;
+    float right_cmd;
+    float left_cmd;
 
-    memcpy( &right_speed, params->data , sizeof(right_speed));
-    memcpy( &left_speed, params->data + sizeof(right_speed), sizeof(left_speed));
+    memcpy( &right_cmd, params->data , sizeof(right_cmd));
+    memcpy( &left_cmd, params->data + sizeof(right_cmd), sizeof(left_cmd));
     
-	wb.SetRPM(left_speed,right_speed);
+	wb.SetRPM(left_cmd,right_cmd);
 #ifdef DEBUG
-	pc.printf("SetRPM %f,%f \n\r",left_speed,right_speed);
+	pc.printf("SetRPM %f,%f \n\r",left_cmd,right_cmd);
 #endif
 }
 
@@ -153,7 +153,10 @@
 		wait_ms(10);
 	}
 #endif
+	wb.control_enable(true);
+	wb.SetRPM(-10,10);
     while (true) {
+    	
     	//センサ値の取得
     	line = wb.GetLinePosition();
     	rrpm = wb.get_right_rpm();
@@ -167,16 +170,16 @@
     	
     	memcpy(mpu_buf,mpudata,sizeof(mpudata));
     	
-    	
+    	//送信
         ble.updateCharacteristicValue(senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf));
         ble.updateCharacteristicValue(mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf));
     	
 		//
 		#ifdef DEBUG
-			pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,rrpm,lrpm);
+			pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,lrpm, rrpm);
 			pc.printf("LinePos:%d ",line);
 			//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",mpudata[0], mpudata[1], mpudata[2], mpudata[3], mpudata[4], mpudata[5]);
+		    pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)",mpudata[0], mpudata[1], mpudata[2], mpudata[3], mpudata[4], mpudata[5]);
 		    pc.printf("\n\r");
 		#endif
 		wait_ms(10);

--- a/wallbotble/wallbotble/wallbotble.cpp	Thu May 17 05:12:34 2018 +0000
+++ b/wallbotble/wallbotble/wallbotble.cpp	Thu May 17 06:11:48 2018 +0000
@@ -69,11 +69,13 @@
 			_calibratedMaximum[i] = 0;
 		}
 		//PIDの初期化
-	    _ctrl_r.setInputLimits(0,100);
-	    _ctrl_l.setInputLimits(0,100); 
-	    
-	    _ctrl_r.setOutputLimits(-1.0, 1.0);
-	    _ctrl_l.setOutputLimits(-1.0, 1.0);
+	    _ctrl_r.setInputLimits(-100,100);
+	    _ctrl_l.setInputLimits(-100,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){
@@ -88,11 +90,29 @@
     _ctrl_l.setProcessValue(get_left_rpm());
 
 	//制御指令
-    _right_motor = _ctrl_r.compute();
-    _left_motor = _ctrl_l.compute();
+	if(_right_back)
+	    _right_motor = -1*_ctrl_r.compute();
+	else
+	    _right_motor = _ctrl_r.compute();
+	if(_left_back)
+		_left_motor = -1*_ctrl_l.compute();
+    else
+	    _right_motor = _ctrl_r.compute();
+    	
 }
 
 void wallbotble::SetRPM(float leftRPM, float rightRPM){
+	_right_back = false;
+	_left_back = false;
+	if(rightRPM < 0){
+		_right_back = true;
+		rightRPM = -1 *rightRPM;
+	}
+	if(leftRPM < 0){
+		_left_back = true;
+		leftRPM = -1 *leftRPM;
+	}
+	
 	_ctrl_l.setSetPoint(leftRPM);
     _ctrl_r.setSetPoint(rightRPM);	
 }
@@ -102,11 +122,7 @@
 	if(_right_timer.read_ms() > 500)
 		_right_rpm = 0;
 	
-	//Duty値が負ならRPMも負
-	if(_right_motor.nowspeed < 0)
-		return -1 * _right_rpm;
-	else
-		return _right_rpm;
+	return _right_rpm;
 }
 
 float wallbotble::get_left_rpm(){
@@ -114,11 +130,7 @@
 	if(_left_timer.read_ms() > 500)
 		_left_rpm = 0;
 		
-	//Duty値が負ならRPMも負
-	if(_right_motor.nowspeed < 0)
-		return -1 * _left_rpm;
-	else
-		return _left_rpm;
+	return _left_rpm;
 }
 
 void wallbotble::right_count(){

--- a/wallbotble/wallbotble/wallbotble.h	Thu May 17 05:12:34 2018 +0000
+++ b/wallbotble/wallbotble/wallbotble.h	Thu May 17 06:11:48 2018 +0000
@@ -33,9 +33,9 @@
 
 #define PulsesPerRev	24
 
-#define Kp  0.1
+#define Kp  0.03
 #define Ki  0.01
-#define Kd  0
+#define Kd  0.001
 #define RATE 0.01 // 制御周期(sec) 0.01=10ms
 
 /** wallbot ble control class
@@ -183,6 +183,10 @@
     unsigned short _left_pulses;
     
 private :
+	//後退用フラグ
+	bool _right_back;
+	bool _left_back;
+	
 	float _right_rpm;
 	float _left_rpm;