2024电赛E题机械臂+cv2视觉方案

曾经的年少青春 · 发表于 2024-9-2 23:10:22

2024电赛E题_机械臂+cv2视觉方案三子棋_人机对弈1.整体设计方案2.机械臂系统方案使用常见的开源六轴自由度stm32机械手臂直接使用商家官方给的代码，我们只需要通过串口给它发送六个舵机的PWM占空比即可控制机械臂的运动通过商家提供的源码，了解它的串口通信协议，仿照通信协议的格式发送数据即可3.视觉系统方案JetsonNano开发板，树莓派也可，都一样用识别内容：黑白棋子共十个的圆心坐标九宫格棋盘的旋转角度（棋盘中心固定，已知旋转角度和棋盘尺寸即可计算九个格的中心坐标）数据发送：棋盘外最下方的黑棋子坐标和白棋子的坐标棋盘九个格的状态(‘X’-黑子,‘O’-白子,’'-空)九宫格九个中心点的位置坐标4.逆运动学解算机器人运动学逆解指的是已知机器人末端执行器在工作空间中的期望位置和姿态，求解出机器人各个关节变量的取值。为了使机械臂能够准确地在棋盘上进行落子操作，需要知道每个关节应如何运动才能达到期望的末端位置和姿态。通过运动学逆解，计算出各个关节的角度或位移，从而实现精确的落子动作。合理的逆解计算可以优化机械臂的运动路径，减少不必要的动作和时间消耗。输入坐标，输出每个轴的角度用到了高中数学，余弦定理，公式为cosC=(a²+b²-c²)/2ab计算步骤已知机械臂末端坐标(x,y,z)和α角，求角首先，测量出机械臂的长度(mm)：L1=105.0L_1=105.0L1=105.0L2=89.0L_2=89.0L2=89.0L3=180.0L_3=180.0L3=180.0计算在xy平面内的底盘舵机角度θ1\theta_1θ1：θ1=arctan⁡(yx)\theta_1=\arctan(\frac{y}{x})θ1=arctan(xy)将三维立体坐标转换到二维平面内：机械臂末端的高度和水平距离：D=∣x2+y2∣D=\sqrt{|x^2+y^2|}D=∣x2+y2∣，H=zH=zH=zL3L_3L3在xxx方向和yyy方向的分量：xα=L3×cos⁡(αrad)x_{\alpha}=L_3\times\cos(\alpha_{rad})xα=L3×cos(αrad)，yα=L3×sin⁡(αrad)y_{\alpha}=L_3\times\sin(\alpha_{rad})yα=L3×sin(αrad)连杆L1L_1L1和L2L_2L2的xxx和yyy分量：xθ=D−xαx_{\theta}=D-x_{\alpha}xθ=D−xα，yθ=H−yαy_{\theta}=H-y_{\alpha}yθ=H−yα连杆L2L_2L2末端到原点的距离:dθ=xθ2+yθ2d_{\theta}=\sqrt{x_{\theta}^2+y_{\theta}^2}dθ=xθ2+yθ2连杆L1L_1L1与L2L_2L2之间的夹角Θ\ThetaΘ：Θ=arccos⁡(L12+L22−dθ22×L1×L2)\Theta=\arccos(\frac{L_1^2+L_2^2-d_{\theta}^2}{2\timesL_1\timesL_2})Θ=arccos(2×L1×L2L12+L22−dθ2)舵机4的角度θ3\theta_3θ3：θ3=180−π×1.5−Θ180\theta_3=180-\frac{\pi\times1.5-\Theta}{180}θ3=180−180π×1.5−Θ（转换为角度）角度θ21\theta_{2_1}θ21和θ22\theta_{2_2}θ22：θ21=arccos⁡(L12+dθ2−L222×L1×dθ)\theta_{2_1}=\arccos(\frac{L_1^2+d_{\theta}^2-L_2^2}{2\timesL_1\timesd_{\theta}})θ21=arccos(2×L1×dθL12+dθ2−L22)，θ22=arccos⁡(xθdθ)\theta_{2_2}=\arccos(\frac{x_{\theta}}{d_{\theta}})θ22=arccos(dθxθ)舵机5的角度θ2\theta_2θ2：θ2=180−(θ21180+θ22180)\theta_2=180-(\frac{\theta_{2_1}}{180}+\frac{\theta_{2_2}}{180})θ2=180−(180θ21+180θ22)舵机3的角度θ4\theta_4θ4：θ4=∣Θ−αrad+θ21+θ22180−90∣\theta_4=|\frac{\Theta-\alpha_{rad}+\theta_{2_1}+\theta_{2_2}}{180}-90|θ4=∣180Θ−αrad+θ21+θ22−90∣5.串口通信协议stm32中控>>>stm32机械臂机械臂源码中接收的数据格式stm32中控发送的数据处理根据源码中定义的数据格式设定串口发送函数//---串口发送--------------------------------------------------------------------------------------//{G0000#000P1500T1000!#001P1500T1000!#002P1500T1000!#003P1500T1000!#004P1500T1000!#005P1500T1000!}//1号p1500t10002号p1500t1000...uint8_thex_buffer[98];voidsend_data_deal(void){ hex_buffer[0]='{'; hex_buffer[1]='G'; hex_buffer[2]='0'; hex_buffer[3]='0'; hex_buffer[4]='0'; hex_buffer[5]='1'; hex_buffer[6]='#'; hex_buffer[7]='0'; hex_buffer[8]='0'; hex_buffer[9]='0'; hex_buffer[10]='P'; hex_buffer[11]=((theta1*11.1+500)/1000)+'0'; hex_buffer[12]=((int)(theta1*11.1+500)%1000)/100+'0'; hex_buffer[13]=((int)(theta1*11.1+500)%100)/10+'0'; hex_buffer[14]=((int)(theta1*11.1+500)%10)+'0';; hex_buffer[15]='T'; hex_buffer[16]=(time_move/1000)+'0'; hex_buffer[17]=(time_move%1000)/100+'0'; hex_buffer[18]=(time_move%100)/10+'0'; hex_buffer[19]=(time_move%10)+'0';; hex_buffer[20]='!'; hex_buffer[21]='#'; hex_buffer[22]='0'; hex_buffer[23]='0'; hex_buffer[24]='1'; hex_buffer[25]='P'; hex_buffer[26]=((theta2*11.1+500)/1000)+'0'; hex_buffer[27]=((int)(theta2*11.1+500)%1000)/100+'0'; hex_buffer[28]=((int)(theta2*11.1+500)%100)/10+'0'; hex_buffer[29]=((int)(theta2*11.1+500)%10)+'0';; hex_buffer[30]='T'; hex_buffer[31]=(time_move/1000)+'0'; hex_buffer[32]=(time_move%1000)/100+'0'; hex_buffer[33]=(time_move%100)/10+'0'; hex_buffer[34]=(time_move%10)+'0';; hex_buffer[35]='!'; hex_buffer[36]='#'; hex_buffer[37]='0'; hex_buffer[38]='0'; hex_buffer[39]='2'; hex_buffer[40]='P'; hex_buffer[41]=((theta3*11.1+500)/1000)+'0'; hex_buffer[42]=((int)(theta3*11.1+500)%1000)/100+'0'; hex_buffer[43]=((int)(theta3*11.1+500)%100)/10+'0'; hex_buffer[44]=((int)(theta3*11.1+500)%10)+'0';; hex_buffer[45]='T'; hex_buffer[46]=(time_move/1000)+'0'; hex_buffer[47]=(time_move%1000)/100+'0'; hex_buffer[48]=(time_move%100)/10+'0'; hex_buffer[49]=(time_move%10)+'0';; hex_buffer[50]='!'; hex_buffer[51]='#'; hex_buffer[52]='0'; hex_buffer[53]='0'; hex_buffer[54]='3'; hex_buffer[55]='P'; hex_buffer[56]=((theta4*11.1+500)/1000)+'0'; hex_buffer[57]=((int)(theta4*11.1+500)%1000)/100+'0'; hex_buffer[58]=((int)(theta4*11.1+500)%100)/10+'0'; hex_buffer[59]=((int)(theta4*11.1+500)%10)+'0';; hex_buffer[60]='T'; hex_buffer[61]=(time_move/1000)+'0'; hex_buffer[62]=(time_move%1000)/100+'0'; hex_buffer[63]=(time_move%100)/10+'0'; hex_buffer[64]=(time_move%10)+'0';; hex_buffer[65]='!'; hex_buffer[66]='#'; hex_buffer[67]='0'; hex_buffer[68]='0'; hex_buffer[69]='4'; hex_buffer[70]='P'; hex_buffer[71]=((theta5*11.1+500)/1000)+'0'; hex_buffer[72]=((int)(theta5*11.1+500)%1000)/100+'0'; hex_buffer[73]=((int)(theta5*11.1+500)%100)/10+'0'; hex_buffer[74]=((int)(theta5*11.1+500)%10)+'0';; hex_buffer[75]='T'; hex_buffer[76]=(time_move/1000)+'0'; hex_buffer[77]=(time_move%1000)/100+'0'; hex_buffer[78]=(time_move%100)/10+'0'; hex_buffer[79]=(time_move%10)+'0';; hex_buffer[80]='!'; hex_buffer[81]='#'; hex_buffer[82]='0'; hex_buffer[83]='0'; hex_buffer[84]='5'; hex_buffer[85]='P'; hex_buffer[86]=((theta6*11.1+500)/1000)+'0'; hex_buffer[87]=((int)(theta6*11.1+500)%1000)/100+'0'; hex_buffer[88]=((int)(theta6*11.1+500)%100)/10+'0'; hex_buffer[89]=((int)(theta6*11.1+500)%10)+'0';; hex_buffer[90]='T'; hex_buffer[91]=(time_move/1000)+'0'; hex_buffer[92]=(time_move%1000)/100+'0'; hex_buffer[93]=(time_move%100)/10+'0'; hex_buffer[94]=(time_move%10)+'0';; hex_buffer[95]='!'; hex_buffer[96]='}'; hex_buffer[97]='\0';}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107JetsonNano>>>stm32中控JetsonNano数据发送将所有要发送的数据整合到一起发送#局外白棋send_data_str="aaab%03d%03d"%(global_white_outside[0],global_white_outside[1])#局外黑棋send_data_str+="%03d%03d"%(global_black_outside[0],global_black_outside[1])#棋局状态send_data_str+="%c%c%c%c%c%c%c%c%c"%(global_game_ststus[0],global_game_ststus[1],global_game_ststus[2],global_game_ststus[3],global_game_ststus[4],global_game_ststus[5],global_game_ststus[6],global_game_ststus[7],global_game_ststus[8])#棋盘九个中心点坐标send_data_str+="%03d%03d"%(global_board_center[0],global_board_center[1])#1send_data_str+="%03d%03d"%(global_board_center[2],global_board_center[3])#2send_data_str+="%03d%03d"%(global_board_center[4],global_board_center[5])#3send_data_str+="%03d%03d"%(global_board_center[6],global_board_center[7])#4send_data_str+="%03d%03d"%(global_board_center[8],global_board_center[9])#5send_data_str+="%03d%03d"%(global_board_center[10],global_board_center[11])#6send_data_str+="%03d%03d"%(global_board_center[12],global_board_center[13])#7send_data_str+="%03d%03d"%(global_board_center[14],global_board_center[15])#8send_data_str+="%03d%03dzzz"%(global_board_center[16],global_board_center[17])#912345678910111213141516stm32中控根据发送的格式设定接收函数//局外黑白棋子坐标intglobal_white_outside[2]={0};intglobal_black_outside[2]={0};//棋局状态charglobal_game_ststus[9]={""};//棋盘九个中心点坐标intglobal_board_center[9][2]={0};//棋盘索引charclass_board_index[3][3]={0,1,2,3,4,5,6,7,8};/*-USART1===================================================================*/charRX_buffer[90]={0};//数据储存charRX_buffer_IT;//接收缓冲intRx_CNT=0;//数组下标/***********串口中断回调**************/voidHAL_UART_RxCpltCallback(UART_HandleTypeDef*huart){ if(huart->Instance==USART1) { if(Rx_CNTRight_boundary):#如果在边界外if(y>global_black_outside[1]):#如果是下面的global_black_outside=(x,y)else:i=0while(iRight_boundary):#如果在边界外if(y>global_white_outside[1]):#如果是下面的global_white_outside=(x,y)else:i=0while(i20):fordata1,numinsend_LvBo_1:if(num>num_lvbo):num_lvbo=numsend_data_str=data1send_LvBo_1.clear()print(send_data_str,'\n')ser.write(send_data_str.encode())#串口发送1234567891011121314151617181920212223242526272829数据过滤在识别过程中经常会出现某一帧识别不到或不准，大部分时间识别是准确的，但是如果将识别不准的这一帧数据发送出去，将会造成许多未知的错误为此，我将数据做以下处理对识别到的圆形的坐标进行储存，每储存15帧便进行检查，将识别到圆形个数最多的一组作为最终数据对识别到的每条线的角度进行储存，将储存的数进行排序，取中间的数值作为最终数据blurred=cv2.GaussianBlur(gray,(5,5),0)#找到圆形-param2:圆形的相似度阈值circles=cv2.HoughCircles(blurred,cv2.HOUGH_GRADIENT,dp=1.1,minDist=30,param1=50,param2=40,minRadius=MIN_RADIUS,maxRadius=MAX_RADIUS)global_circles_count=0ifcirclesisnotNone:goable_circle_centers_black.clear()#清空数组goable_circle_centers_white.clear()#清空数组global_circles_count=len(circles[0,:])#获得圆形个数circles=np.round(circles[0,:]).astype("int")for(x,y,r)incircles:color=get_color(frame[y,x][0],frame[y,x][1],frame[y,x][2])cv2.circle(frame,(x,y),r,(0,255,0),4)cv2.putText(frame,color,(x-r,y-r),cv2.FONT_HERSHEY_SIMPLEX,0.6,(0,0,255),2)#将中心坐标添加到列表中if(color=='B'):goable_circle_centers_black.append((x,y))global_black_outside=(0,0)if(color=='W'):goable_circle_centers_white.append((x,y))global_white_outside=(0,0)global_game_ststus=['','','','','','','','','']#找到最下面的边界外的棋子for(x,y)ingoable_circle_centers_black:#黑if(xRight_boundary):#如果在边界外if(y>global_black_outside[1]):#如果是下面的global_black_outside=(x,y)else:i=0while(iRight_boundary):#如果在边界外if(y>global_white_outside[1]):#如果是下面的global_white_outside=(x,y)else:i=0while(i20):fordata1,numinsend_LvBo_1:if(num>num_lvbo):num_lvbo=numsend_data_str=data1send_LvBo_1.clear()print(send_data_str,'\n')ser.write(send_data_str.encode())#串口发送1234567891011全部代码importcv2importnumpyasnpimportmathimportserialser=serial.Serial("/dev/ttyTHS1",115200,timeout=0.01)print("serialOpen")#全局变量定义=========================================================global_angle=0global_angle_lvbo=[]width=0global_width_mid=(width//2)height=0global_height_mid=(height//2)send_LvBo_1=[]#局外黑白棋子global_white_outside=(0,0)global_black_outside=(0,0)#棋局状态global_game_ststus=['','','','','','','','','']#棋局九个中心坐标global_board_center=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]#圆形坐标goable_circle_centers_black=[]goable_circle_centers_white=[]global_circles_count=0#---------------------------------------------------------------------defcalculate_distance(point1,point2):"""计算两点之间的距离。参数：point1(tuple):第一个点的坐标，格式为(x1,y1)。point2(tuple):第二个点的坐标，格式为(x2,y2)。返回：float:两点之间的距离。"""x1,y1=point1x2,y2=point2distance=math.sqrt((x2-x1)**2+(y2-y1)**2)returndistancedefdraw_rotating_grid(frame,center,size,angle):"""在图像上绘制旋转的九宫格并在格子中心标注十字，同时不显示内部四条线，并且储存并打印九个中心点坐标，给每个格子标上序号:paramframe:图像帧:paramcenter:九宫格中心点坐标(center_x,center_y):paramsize:九宫格中每个小方格的边长:paramangle:旋转角度，单位为度"""defcalculate_rotated_point(x,y,cx,cy,cos_angle,sin_angle):temp_x=x-cxtemp_y=y-cynew_x=temp_x*cos_angle-temp_y*sin_angle+cxnew_y=temp_x*sin_angle+temp_y*cos_angle+cyreturnint(new_x),int(new_y)defcalculate_grid_corners(center,size,angle):half_size=size*1.5#因为3x3格子有3个边长angle_rad=math.radians(angle)cos_angle=math.cos(angle_rad)sin_angle=math.sin(angle_rad)points=[]foriinrange(4):forjinrange(4):x=center[0]+(i-1.5)*sizey=center[1]+(j-1.5)*sizerotated_point=calculate_rotated_point(x,y,center[0],center[1],cos_angle,sin_angle)points.append(rotated_point)returnpoints#计算旋转后的九宫格顶点points=calculate_grid_corners(center,size,angle)#绘制九宫格的外部线条#画上下两条外部线cv2.line(frame,points[0],points[3],(0,255,0),2)cv2.line(frame,points[12],points[15],(0,255,0),2)#画左右两条外部线cv2.line(frame,points[0],points[12],(0,255,0),2)cv2.line(frame,points[15],points[3],(0,255,0),2)#计算并储存每个格子的中心坐标grid_centers=[]foriinrange(3):forjinrange(3):#计算格子中心坐标（这里的计算基于九宫格的布局规律）grid_center_x=center[0]+(i-1)*sizegrid_center_y=center[1]+(j-1)*sizeangle_rad=math.radians(angle)cos_angle=math.cos(angle_rad)sin_angle=math.sin(angle_rad)#计算旋转后的格子中心坐标rotated_grid_center_x,rotated_grid_center_y=calculate_rotated_point(grid_center_x,grid_center_y,center[0],center[1],cos_angle,sin_angle)grid_centers.append((rotated_grid_center_x,rotated_grid_center_y))#打印九个中心点坐标globalglobal_board_centerfori,center_coordinenumerate(grid_centers):if(i==0):global_board_center[0]=center_coord[0]global_board_center[1]=center_coord[1]elif(i==1):global_board_center[2]=center_coord[0]global_board_center[3]=center_coord[1]elif(i==2):global_board_center[4]=center_coord[0]global_board_center[5]=center_coord[1]elif(i==3):global_board_center[6]=center_coord[0]global_board_center[7]=center_coord[1]elif(i==4):global_board_center[8]=center_coord[0]global_board_center[9]=center_coord[1]elif(i==5):global_board_center[10]=center_coord[0]global_board_center[11]=center_coord[1]elif(i==6):global_board_center[12]=center_coord[0]global_board_center[13]=center_coord[1]elif(i==7):global_board_center[14]=center_coord[0]global_board_center[15]=center_coord[1]elif(i==8):global_board_center[16]=center_coord[0]global_board_center[17]=center_coord[1]#print(f"格子{i+1}的中心点坐标:{center_coord}")#绘制十字并标注序号fori,center_coordinenumerate(grid_centers):cross_size=int(size/8)cv2.line(frame,(center_coord[0]-cross_size,center_coord[1]),(center_coord[0]+cross_size,center_coord[1]),(255,0,0),1)cv2.line(frame,(center_coord[0],center_coord[1]-cross_size),(center_coord[0],center_coord[1]+cross_size),(255,0,0),1)#在格子中心标注序号cv2.putText(frame,str(i+1),(center_coord[0]-5,center_coord[1]+5),cv2.FONT_HERSHEY_SIMPLEX,0.5,(0,0,255),2)#----------------------------------------------------------------------------------------------------------------------------------------------#----------------------------------------------------------------------------------------------------------------------------------------------#----------------------------------------------------------------------------------------------------------------------------------------------#----------------------------------------------------------------------------------------------------------------------------------------------#----------------------------------------------------------------------------------------------------------------------------------------------#----------------------------------------------------------------------------------------------------------------------------------------------#----------------------------------------------------------------------------------------------------------------------------------------------data_send="123"#ser.write(data_send.encode())#打开摄像头cap=cv2.VideoCapture(0)#定义一些常量enable_lens_corr=False#打开以获得更直的线条min_degree=2max_degree=179MIN_RADIUS=25#圆形最小半径MAX_RADIUS=50#圆形最大半径#镜头畸变校正defcorrect_lens_distortion(image):h,w=image.shape[:2]k=np.array([[w,0,w/2],[0,h,h/2],[0,0,1]],dtype=np.float32)dist=np.zeros((5,),dtype=np.float32)corrected_image=cv2.undistort(image,k,dist)returncorrected_imagedefget_color(b,g,r):#定义颜色阈值white_threshold=(150,150,150)black_threshold=(104,86,121)#判断颜色ifr>white_threshold[2]andg>white_threshold[1]andb>white_threshold[0]:return"W"elifrRight_boundary):#如果在边界外if(y>global_black_outside[1]):#如果是下面的global_black_outside=(x,y)else:i=0while(iRight_boundary):#如果在边界外if(y>global_white_outside[1]):#如果是下面的global_white_outside=(x,y)else:i=0while(i20):fordata1,numinsend_LvBo_1:if(num>num_lvbo):num_lvbo=numsend_data_str=data1send_LvBo_1.clear()print(send_data_str,'\n')ser.write(send_data_str.encode())#串口发送cv2.imshow('Frame',frame)ifcv2.waitKey(1)&0xFF==ord('q'):breakcap.release()cv2.destroyAllWindows()1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503517.主控程序设计OLED屏幕显示主菜单voidOled_menu(void){ charlode_text[40]={0}; sprintf(lode_text,"---MENU---"); OLED_ShowString(0,8,(uint8_t*)lode_text,8,1); sprintf(lode_text,"K1:Topic_1"); OLED_ShowString(0,16,(uint8_t*)lode_text,8,1); sprintf(lode_text,"K2:Topic_2/3"); OLED_ShowString(0,24,(uint8_t*)lode_text,8,1); sprintf(lode_text,"K3:Topic_4"); OLED_ShowString(0,32,(uint8_t*)lode_text,8,1); sprintf(lode_text,"K4:Topic_5/6"); OLED_ShowString(0,40,(uint8_t*)lode_text,8,1); sprintf(lode_text,""); OLED_ShowString(0,48,(uint8_t*)lode_text,8,1); sprintf(lode_text,"key:%02d%02d%02d%02d%02d",Key_Down_Num[0],Key_Down_Num[1],Key_Down_Num[2],Key_Down_Num[3],Key_Down_Num[4]); OLED_ShowString(0,56,(uint8_t*)lode_text,8,1); OLED_Refresh();}123456789101112131415161718192021222324252627第一题界面voidOled_Topic_1(void){ charlode_text[40]={0}; sprintf(lode_text,"black_out

%d,%d)",coordinate_transformat_x(global_black_outside[0]),coordinate_transformat_y(global_black_outside[1])); OLED_ShowString(0,8,(uint8_t*)lode_text,8,1); sprintf(lode_text,"k1:-----"); OLED_ShowString(0,16,(uint8_t*)lode_text,8,1); sprintf(lode_text,"k2:OK"); OLED_ShowString(0,24,(uint8_t*)lode_text,8,1); sprintf(lode_text,"k3:-----"); OLED_ShowString(0,32,(uint8_t*)lode_text,8,1); sprintf(lode_text,"k4:break"); OLED_ShowString(0,40,(uint8_t*)lode_text,8,1); sprintf(lode_text,"key:%02d%02d%02d%02d%02d",Key_Down_Num[0],Key_Down_Num[1],Key_Down_Num[2],Key_Down_Num[3],Key_Down_Num[4]); OLED_ShowString(0,56,(uint8_t*)lode_text,8,1); OLED_Refresh();}12345678910111213141516171819202122232425第二、三题界面voidOled_Topic_2(void){ charlode_text[40]={0}; sprintf(lode_text,"choose_xy

%d,%d)",coordinate_transformat_x(global_board_center[class_board_index[x_chose][y_chose]][0]+5),coordinate_transformat_y(global_board_center[class_board_index[x_chose][y_chose]][1])-10); OLED_ShowString(0,8,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%c1%c1%c%c1%c1%ck1:%s",chessBoard_chose[0][0],chessBoard_chose[0][1],chessBoard_chose[0][2],global_game_ststus[0],global_game_ststus[1],global_game_ststus[2],point_key0_text);OLED_ShowString(0,16,(uint8_t*)lode_text,8,1);sprintf(lode_text,"----------k2:%s",point_key1_text);OLED_ShowString(0,24,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%c1%c1%c%c1%c1%ck3:%s",chessBoard_chose[1][0],chessBoard_chose[1][1],chessBoard_chose[1][2],global_game_ststus[3],global_game_ststus[4],global_game_ststus[5],point_key2_text);OLED_ShowString(0,32,(uint8_t*)lode_text,8,1);sprintf(lode_text,"----------k4:%s",point_key3_text);OLED_ShowString(0,40,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%c1%c1%c%c1%c1%c",chessBoard_chose[2][0],chessBoard_chose[2][1],chessBoard_chose[2][2],global_game_ststus[6],global_game_ststus[7],global_game_ststus[8]);OLED_ShowString(0,48,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%s",point_text);OLED_ShowString(0,56,(uint8_t*)lode_text,8,1);OLED_Refresh();}1234567891011121314151617181920第四、五、六题界面voidprint_board(void){charlode_text[40]={0}; sprintf(lode_text,"PlayerVSRobot");OLED_ShowString(0,8,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%c1%c1%ck1:%s",board[0][0],board[0][1],board[0][2],point_key0_text);OLED_ShowString(0,16,(uint8_t*)lode_text,8,1);sprintf(lode_text,"-----k2:%s",point_key1_text);OLED_ShowString(0,24,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%c1%c1%ck3:%s",board[1][0],board[1][1],board[1][2],point_key2_text);OLED_ShowString(0,32,(uint8_t*)lode_text,8,1);sprintf(lode_text,"-----k4:%s",point_key3_text);OLED_ShowString(0,40,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%c1%c1%c",board[2][0],board[2][1],board[2][2]);OLED_ShowString(0,48,(uint8_t*)lode_text,8,1);sprintf(lode_text,"%s",point_text);OLED_ShowString(0,56,(uint8_t*)lode_text,8,1);OLED_Refresh();}1234567891011121314151617181920棋盘逻辑没有用minmax算法，采用优先级判断判断是否结束判断是否有赢的机会判断是否有堵的机会判断中心方格是否为空判断四个角的方格是否为空判断四个边的方格是否为空//将接收到的棋盘信息储存voidcheck_board(void){ board[0][0]=global_game_ststus[0]; board[0][1]=global_game_ststus[1]; board[0][2]=global_game_ststus[2]; board[1][0]=global_game_ststus[3]; board[1][1]=global_game_ststus[4]; board[1][2]=global_game_ststus[5]; board[2][0]=global_game_ststus[6]; board[2][1]=global_game_ststus[7]; board[2][2]=global_game_ststus[8];}//检查是否平局boolcheck_draw(){ check_board();for(inti=0;i#include#definePI3.141#defineL1105.0#defineL298.0#defineL3150.0//底座...夹爪doubletheta1=90,theta2=90,theta3=90,theta4=90,theta5=0,theta6=50;//xyz坐标+末端位姿alphaintx1=0,y1=0,z1=374,alpha=90;inttime_move=1000;//弧度转角度doubleRad2Deg(doublerad){returnrad*180/PI;}//角度转弧度doubleDeg2Rad(doubledeg){returndeg*PI/180;}//计算底盘舵机角度，并将坐标转换到二维boolJiSuan(doublex,doubley,doublez,doublealpha){ //计算底盘舵机角度theta1=(atan2(y,x)*180/PI)-45;doubleH=z;//高度doubleD=sqrt(fabs(x*x)+y*y);//水平距离returnMy_Model(D,H,alpha);}//在二维平面计算角度boolMy_Model(doubleD,doubleH,doublealpha){alpha=Deg2Rad(alpha);//转为弧度doublex_alpha=L3*cos(alpha);doubley_alpha=L3*sin(alpha);doublex_theta=D-x_alpha;doubley_theta=H-y_alpha;doubled_theta=sqrt(x_theta*x_theta+y_theta*y_theta);if(d_theta>(L1+L2)||d_theta=0&theta1=0&theta2=0&theta3=0&theta4230&x_input230&x_input>0)//右上角 { y_input-=(y_input*0.020); } if(x_input>0)//右侧 { x_input+=(x_input)*0.25-y_input*0.009; } if(y_input0)//右下角 { y_input+=(230-y_input)*0.15; x_input+=abs(230-y_input)*0.2; } //xy上方 theta6=50; x1=x_input; y1=y_input; z1=Z_START; alpha=A_START; if(y_input>230&x_input>0) z1-=5; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000); //xyz x1=x_input;//-50 y1=y_input;//-55 z1=Z_DOWN-9+abs(x1)*y1*0.0005; //------误差调整----------------------------- if(x_input220&x_input220&x_input220&x_input230&x_input>40)//右上角 { z1-=2; } if(y_input>260&x_input>40)//右上角上部 { z1+=6; x1-=9; y1-=4; } if(y_input40)//右下角 { z1+=2; x1-=1; y1-=1; } if(y_input40)//右下角下部 y1-=5; alpha=A_START; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1500); //抓上 theta6=110; send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1500); x1=0; y1=170; z1=Z_START+15; alpha=A_START; JiSuan(x1,y1,z1,alpha); send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(500); }voidarm_FangXia(intx_input,inty_input){ time_move=1000; //xy上方 x1=x_input; y1=y_input-10; z1=Z_START; alpha=A_START; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000); //xyz x1=x_input; y1=y_input; z1=Z_DOWN+10; alpha=A_START; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000); //放下 theta6=90; send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000); //xy上方 x1=x_input; y1=y_input; z1=Z_START; alpha=A_START; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000); //初始位置 x1=X_START; y1=Y_START; z1=Z_START; alpha=A_START; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000);}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177第六题思路intBuGaiZai[2]={-1,-1};//不该在这intYingGaiZai[2]={-1,-1};//应该在这voidCheck_Board_Legal_index(charplayer,intx_input,inty_input,intgolobal_num){ charvs_player; if(player==BLACK) vs_player=WHITE; else vs_player=BLACK; if(Board_Legal[x_input][y_input]!=global_game_ststus[golobal_num]) { if((Board_Legal[x_input][y_input]=='')&(global_game_ststus[golobal_num]!=vs_player))//应该是空，不该在这 { BuGaiZai[0]=x_input; BuGaiZai[1]=y_input; } if(Board_Legal[x_input][y_input]==player)//应该是player，应该在这 { YingGaiZai[0]=x_input; YingGaiZai[1]=y_input; } } }intCheck_Board_Legal_1(charplayer){ //初始化 BuGaiZai[0]=-1; BuGaiZai[1]=-1; YingGaiZai[0]=-1; YingGaiZai[1]=-1; //全部检查 Check_Board_Legal_index(player,0,0,0); Check_Board_Legal_index(player,0,1,1); Check_Board_Legal_index(player,0,2,2); Check_Board_Legal_index(player,1,0,3); Check_Board_Legal_index(player,1,1,4); Check_Board_Legal_index(player,1,2,5); Check_Board_Legal_index(player,2,0,6); Check_Board_Legal_index(player,2,1,7); Check_Board_Legal_index(player,2,2,8); if(BuGaiZai[0]!=-1&YingGaiZai[0]!=-1) { //------误差调整----------------------------- intdata_x=0,data_y=0; if(BuGaiZai[0]==0&BuGaiZai[1]==1)//中上 { data_x=4; } if(BuGaiZai[0]==1&BuGaiZai[1]==1)//中间 { data_x=5; } if(BuGaiZai[0]==0&BuGaiZai[1]==0)//右上 { data_x=9; } if(BuGaiZai[0]==1&BuGaiZai[1]==0)//右中 { data_x=8; } if(BuGaiZai[0]==2&BuGaiZai[1]==0)//下 { data_y=-9; } if(BuGaiZai[0]==2&BuGaiZai[1]==1)//下 { data_y=-9; } if(BuGaiZai[0]==2&BuGaiZai[1]==2)//下 { data_y=-12; } //抓不该，放应该 HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET); arm_JiaQv_Legal(coordinate_transformat_x(global_board_center[class_board_index[BuGaiZai[0]][BuGaiZai[1]]][0]+5)+data_x,coordinate_transformat_y(global_board_center[class_board_index[BuGaiZai[0]][BuGaiZai[1]]][1]+5)+data_y); HAL_Delay(1000); arm_FangXia(coordinate_transformat_x(global_board_center[class_board_index[YingGaiZai[0]][YingGaiZai[1]]][0]+5),coordinate_transformat_y(global_board_center[class_board_index[YingGaiZai[0]][YingGaiZai[1]]][1])-10); HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET); HAL_Delay(1000); } if(BuGaiZai[0]!=-1&YingGaiZai[0]==-1) { //抓不该 } if(BuGaiZai[0]==-1&YingGaiZai[0]!=-1) { //放应该 } return0;} voidarm_JiaQv_Legal(intx_input,inty_input){ time_move=1000; if(x_input230&x_input230&x_input>0)//右上角 { y_input-=(y_input*0.020); } if(x_input>0)//右侧 { x_input+=(x_input)*0.25-y_input*0.009; } if(y_input0)//右下角 { y_input+=(230-y_input)*0.15; x_input+=abs(230-y_input)*0.2; } //xy上方 theta6=80; x1=x_input; y1=y_input; z1=Z_START; alpha=A_START; if(y_input>230&x_input>0) z1-=5; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1000); //xyz x1=x_input;//-50 y1=y_input;//-55 z1=Z_DOWN-9+abs(x1)*y1*0.0005; if(x_input220&x_input220&x_input230&x_input>40)//右上角 { z1-=2; } if(y_input>265&x_input>40)//右上角上部 { z1+=6; } if(y_input40)//右下角 { z1+=2; x1-=1; y1-=1; } z1+=8; alpha=A_START; JiSuan(x1,y1,z1,alpha);//计算并传值给theta send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1500); //抓上 theta6=110; send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(1500); x1=0; y1=170; z1=Z_START+15; alpha=A_START; JiSuan(x1,y1,z1,alpha); send_data_deal();//转换发送的数据 HAL_UART_Transmit(&huart1,(uint8_t*)hex_buffer,97,10);//发送数据 HAL_Delay(500);}123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208

		自动登录	找回密码
密码			会员注册