track speed limit can be set by upper computer

ArduSub/ArduSub.cpp

@@ -103,14 +103,7 @@ void Sub::fast_loop()
     // update INS immediately to get current gyro data populated
     ins.update();
 
-	float depth = barometer.get_altitude(); // cm
-	static int16_t c4  = 0;
-		c4++;
-		if(c4>400){
-			c4 =0 ;
-			gcs().send_text(MAV_SEVERITY_INFO, "depth %f.",(float)depth);
-			
-		}
+	
 
     //don't run rate controller in manual or motordetection modes
     if (control_mode != MANUAL && control_mode != MOTOR_DETECT&& control_mode != CLEAN) {

ArduSub/control_clean.cpp

@@ -1,7 +1,7 @@
 #include "Sub.h"
 //目标姿态
 
-float maxspeed = 41.0;
+float maxspeed = 51.0;
 int16_t minspeed = 31;
 int16_t maxerrorspeed = 31;
 
@@ -143,6 +143,8 @@ void Sub::autoclean_flag_chose(void){
 	}
 
 }
+int16_t maxspeed_up=50;
+
 void Sub::clean_net_joystick(void)
 {
 	//左右履带和左右电机 左右反了
@@ -152,8 +154,13 @@ void Sub::clean_net_joystick(void)
 	int16_t motors1 =startval;
 	int16_t motors2 =startval;
 	
-	minspeed =SRV_Channels::srv_channel(11)->get_output_min();// 暂时用来做最小速度   -----最小水深
-	maxerrorspeed =SRV_Channels::srv_channel(11)->get_output_max();//两个履带最大差速 ----------最深距离 厘米 
+	minspeed =SRV_Channels::srv_channel(11)->get_output_min()/100;// 暂时用来做最小速度   -----最小水深
+	maxerrorspeed =SRV_Channels::srv_channel(11)->get_output_max()/100;//两个履带最大差速 ----------最深距离 厘米 
+	maxspeed_up = SRV_Channels::srv_channel(15)->get_trim()/100;
+	if(maxspeed_up<maxspeed)
+	{
+		maxspeed = maxspeed_up;
+	}
 	
 	if(handclean == TRUE ){
 		
@@ -229,16 +236,19 @@ void Sub::clean_net_joystick(void)
 		//motors2=constrain_int16(motors2,-(Speedmax_hand+maxerror),(int16_t)(Speedmax_hand+maxerror));//做履带
 		//motors1=constrain_int16(motors1,-((int16_t)maxspeed+maxerror),(int16_t)((int16_t)maxspeed+maxerror));
 		//motors2=constrain_int16(motors2,-((int16_t)maxspeed+maxerror),(int16_t)((int16_t)maxspeed+maxerror));//做履带
-		motors1=constrain_int16(motors1,-((int16_t)(maxspeed)/2),(int16_t)((int16_t)maxspeed+maxerror));
-		motors2=constrain_int16(motors2,-((int16_t)(maxspeed)/2),(int16_t)((int16_t)maxspeed+maxerror));//做履带
-	
+
+		//motors1=constrain_int16(motors1,-((int16_t)(maxspeed)/2),(int16_t)((int16_t)maxspeed+maxerror));
+		//motors2=constrain_int16(motors2,-((int16_t)(maxspeed)/2),(int16_t)((int16_t)maxspeed+maxerror));//做履带
+		int16_t minspeedlimit =(int16_t)(maxspeed*0.5);
+		motors1=constrain_int16(motors1,-(int16_t)minspeedlimit,(int16_t)((int16_t)maxspeed+maxerror));
+		motors2=constrain_int16(motors2,-(int16_t)minspeedlimit,(int16_t)((int16_t)maxspeed+maxerror));//做履带
 		
 		static int f = 0;
 		f++;
 		if(f>800)
 		{
 			
-			//gcs().send_text(MAV_SEVERITY_INFO, "rov_message.left_brush %d %d,%f",(int)rov_message.left_brush,(int)rov_message.right_brush,maxspeed);
+			gcs().send_text(MAV_SEVERITY_INFO, "minspeed %d %d,%d,%d",(int)minspeed,(int)maxerrorspeed,(int)maxspeed,(int)minspeedlimit);
 			f=0;
 		}
 

ArduSub/joystick.cpp

@@ -660,39 +660,39 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 			if(PressLevel == first)
 			{
 				PressLevel = second;
-				 PressLevel_f = 1.1;//1.1
+				 PressLevel_f = 0.5;//1.1
 			}else if(PressLevel == second){
 				PressLevel = third;
-				PressLevel_f = 1.4;//1.4
+				PressLevel_f = 0.75;//1.4
 			   
 			}else if(PressLevel == third){
 				PressLevel = forth;
-				PressLevel_f = 1.8;//1.8
+				PressLevel_f = 1.0;//1.8
 			}else if (PressLevel == forth){
 				PressLevel = fifth;
-				PressLevel_f = 2.5;//2.5
+				PressLevel_f = 2.75;//2.5
 			}else if(PressLevel == fifth){
 				PressLevel = no;
 				PressLevel_f = 5.0;	//5.0
 			}else if(PressLevel == no){
 				PressLevel = sixth;
-				PressLevel_f = 7.5;//7.5
+				PressLevel_f = 7.25;//7.5
 			}else if (PressLevel == sixth){
 				PressLevel = seventh;
-				PressLevel_f = 8.2;//8.2
+				PressLevel_f = 9.0;//8.2
 			}else if(PressLevel == seventh){
 				PressLevel = eighth;
-				PressLevel_f = 8.6;//8.6
+				PressLevel_f = 9.25;//8.6
 			}else if (PressLevel == eighth){
 				PressLevel = ninth;
-				PressLevel_f = 8.9;//8.9
+				PressLevel_f = 9.5;//8.9
 				
 			}else if (PressLevel == ninth){
 				PressLevel = tenth;
-				PressLevel_f = 10.0;//9.1
+				PressLevel_f = 9.75;//9.1
 			}else{
 				PressLevel = tenth;
-				PressLevel_f = 10.0;//9.1
+				PressLevel_f = 9.75;//9.1
 			}
 			gcs().send_text(MAV_SEVERITY_INFO,"%f level",(float)PressLevel_f);
 		}
@@ -703,37 +703,37 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
 		 if(PressLevel == tenth)
 			 {
 				 PressLevel = ninth;
-				 PressLevel_f = 8.9;//8.9
+				 PressLevel_f = 9.5;//8.9
 			 }else if(PressLevel == ninth) {
 				 PressLevel = eighth;
-				 PressLevel_f = 8.6;//8.6
+				 PressLevel_f = 9.25;//8.6
 			 }else if(PressLevel == eighth){
 				 PressLevel = seventh;
-				 PressLevel_f = 8.2;//8.2
+				 PressLevel_f = 9.0;//8.2
 			 }else if (PressLevel == seventh){
 				 PressLevel = sixth;
-				  PressLevel_f = 7.5;//7.5
+				  PressLevel_f = 7.25;//7.5
 			 }else if(PressLevel == sixth){
 				 PressLevel = no;
 				  PressLevel_f = 5.0;//5.0
 			 }else if(PressLevel == no){
 				 PressLevel = fifth;
-				 PressLevel_f = 2.5; //2.5
+				 PressLevel_f = 2.75; //2.5
 			 }else if (PressLevel == fifth){
 				 PressLevel = forth;
-				  PressLevel_f = 1.8;//1.8	 
+				  PressLevel_f = 1.0;//1.8	 
 			 }else if(PressLevel == forth){
 				 PressLevel = third;
-				  PressLevel_f = 1.4;//1.4
+				  PressLevel_f = 0.75;//1.4
 			 }else if (PressLevel == third){
 				 PressLevel = second;
-				  PressLevel_f = 1.1;	//1.1
+				  PressLevel_f = 0.5;	//1.1
 			 }else if (PressLevel == second){
 				 PressLevel = first;
-				  PressLevel_f = 0.0;//0.9	 
+				  PressLevel_f = 0.25;//0.9	 
 			 }else{
 				 PressLevel = first;
-				  PressLevel_f = 0.0; //0.9
+				  PressLevel_f = 0.25; //0.9
 			 }	 
 			 gcs().send_text(MAV_SEVERITY_INFO,"%f level",(float)PressLevel_f);
 		}

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -422,7 +422,7 @@ void AP_Motors6DOF::output_to_Dshot()
 		if(k>400)
 		{
 
-			gcs().send_text(MAV_SEVERITY_INFO, "motor_out %d %d %d %d\n", (int)mav_motor_speed.motorTest,(int)motor_to_can[2],(int)motor_to_can[3], (int)motor_to_can[4]);
+			//gcs().send_text(MAV_SEVERITY_INFO, "motor_out %d %d %d %d\n", (int)mav_motor_speed.motorTest,(int)motor_to_can[2],(int)motor_to_can[3], (int)motor_to_can[4]);
 			k=0;
 		}
 }

readme.txt

@@ -1 +1,3 @@
-modules\mavlink\pymavlink\generator\C 这个C文件推不到远程端 所以clone以后要手动复制过来才能编译
+modules\mavlink\pymavlink\generator\C 这个C文件推不到远程端 所以clone以后要手动复制过来才能编译
+
+履带最大最小速度可以上位机设置