基于stm32的光点运动轨迹控制

前言

步进电机画圆、直线。使用的简单的模拟脉冲方式快速实现的步进电机运动控制，如有不足的地方欢迎补充。

任务分析

1.任务
制作一个激光笔点二维控制装置，示意如图所示。在50cm*50cm靶纸上，用激光笔投射一光点，激光笔距离靶纸1米。要求能按指定的误差范围将光点定位在靶纸上任意一点，并在限定的条件下将光点按指定轨迹运动。

2.设备方案
主控采用stm32f411retx。控制装置为二自由度旋转式云台。使用42步进电机以及TB6600电机驱动器进行32细分。
3.基本原理
直线运动插补与圆弧运动插补采用的逐点比较法，这里只要注意一点就是激光笔是在云台上进行控制，控制x轴与y轴的电机转动时，需根据实际定位的坐标换算成电机当前相对初始状态要转动的角度。这里让激光笔初始垂直照射到原点位置，如下图。

tan(angle_x) = $x/op$。
tan(angle_y) = $y/\sqrt{(}{x}^2+o{p}^2)$。
使用math.h里的atan可以算出angle值
angle_x = $atan(x/op)\ast 180/PI$;
angle_y = $atan(y/\sqrt{(}{x}^2+o{p}^2))\ast 180/PI$。

4.关键代码

#define Step_one 1 //单步步进值
static float now_x_step = 0, now_y_step = 0;  //记忆当前步数/** x* CCW对应x轴正方向*/
void stepper_x_run(int tim,float step,float subdivide,uint8_t dir)
{int i;if(step < 0.5)return;if(dir == CW)MOTOR_X_DIR(CW);else if(dir == CCW)MOTOR_X_DIR(CCW);osDelay(2);for(i = 0; i < step; i++){if(dir == CW)now_x_step--;else if(dir == CCW)now_x_step++;MOTOR_X_PUL(HIGH);osDelay(tim / 2);MOTOR_X_PUL(LOW);osDelay(tim / 2);}
}
/** y* CW对应y轴正方向*/
void stepper_y_run(int tim, float step, float subdivide, uint8_t dir)
{int i;if(step < 0.5)return;if(dir == CW)MOTOR_Y_DIR(CW);else if(dir == CCW)MOTOR_Y_DIR(CCW);osDelay(2);for(i = 0; i < step; i++){if(dir == CW)now_y_step++;else if(dir == CCW)now_y_step--;MOTOR_Y_PUL(HIGH);osDelay(tim / 2);MOTOR_Y_PUL(LOW);osDelay(tim / 2);}
}
/**
*定点函数
*/
void turn_coordinate(float x, float y)
{float angle_x, angle_y;float step_x, step_y;float dx, dy;float sqx;arm_sqrt_f32(1050 * 1050 + x * x, &sqx);angle_x = atan(x / 1050) * 180 / PI;angle_y = atan(y / sqx) * 180 / PI;step_x = angle_x / 0.05625;//计算对应步数,与0相差step_y = angle_y / 0.05625;dx = step_x - now_x_step;dy = step_y - now_y_step;if(dx > 0)stepper_x_run(2, dx, 32, CCW);else if(dx < 0)stepper_x_run(2, -dx, 32, CW);if(dy > 0)stepper_y_run(2, dy, 32, CW);else if(dy < 0)stepper_y_run(2, -dy, 32, CCW);
}
/** @brief：直线运动插补* @parameter：起点坐标（X0, Y0），终点坐标（Xe, Ye）* @return： 无* */
void drawline(float X0, float Y0, float Xe, float Ye)
{float NXY;              //总步数float Fm = 0;           //偏差float Xm = X0, Ym = Y0; //当前坐标uint8_t XOY;            //象限Xe = Xe - X0;Ye = Ye - Y0;NXY = (fabsf(Xe) + fabsf(Ye)) / Step_one;if(Xe > 0 && Ye >= 0) XOY = 1;else if(Xe <= 0 && Ye > 0) XOY = 2;else if(Xe < 0 && Ye <= 0) XOY = 3;else if(Xe >= 0 && Ye < 0) XOY = 4;while(NXY > 0){switch (XOY){case 1: (Fm >= 0) ? (Xm += Step_one) : (Ym += Step_one); break;case 2: (Fm <  0) ? (Xm -= Step_one) : (Ym += Step_one); break;case 3: (Fm >= 0) ? (Xm -= Step_one) : (Ym -= Step_one); break;case 4: (Fm <  0) ? (Xm += Step_one) : (Ym -= Step_one); break;default: break;}NXY -= 1;Fm = (Ym - Y0) * Xe - (Xm - X0) * Ye;turn_coordinate(Xm, Ym);osDelay(2);}
}
/** @brief：圆运动插补* @parameter：圆心坐标（x0, y0），半径 R, 方向 SorN 1 顺时针 2 逆时针* @return： 无* */
void drawcircle(float x0, float y0, float R, uint8_t SorN)
{float X0, Y0, Xe, Ye;float step = 0;float Fm = 0;float Xm, Ym;uint8_t XOY;X0 = x0; Y0 = y0 + R;  //开始点Xe = x0; Ye = y0 + R;  //结束点Xm = X0; Ym = Y0;while (pow((Xm - Xe), 2) + pow((Ym - Ye), 2) > Step_one * Step_one / 2 || (step == 0)){if ((Xm - x0) > 0 && (Ym - y0) >= 0) XOY = 1;else if ((Xm - x0) <= 0 && (Ym - y0) > 0) XOY = 2;else if ((Xm - x0) < 0 && (Ym - y0) <= 0) XOY = 3;else if ((Xm - x0) >= 0 && (Ym - y0) < 0) XOY = 4;switch (XOY){case 1:if(SorN == 1)(Fm >= 0) ? (Ym -= Step_one) : (Xm += Step_one);else(Fm <= 0) ? (Ym += Step_one) : (Xm -= Step_one);break;case 2:if(SorN == 1)(Fm >= 0) ? (Xm += Step_one) : (Ym += Step_one);else(Fm >  0) ? (Ym -= Step_one) : (Xm -= Step_one);break;case 3:if(SorN == 1)(Fm >= 0) ? (Ym += Step_one) : (Xm -= Step_one);else(Fm >  0) ? (Xm += Step_one) : (Ym -= Step_one);break;case 4:if(SorN == 1)(Fm >= 0) ? (Xm -= Step_one) : (Ym -= Step_one);else(Fm >  0) ? (Ym += Step_one) : (Xm += Step_one);default: break;}step = step + 1;Fm = pow((Xm - x0), 2) + pow((Ym - y0), 2) - pow(R, 2);turn_coordinate(Xm, Ym);osDelay(2);}
}

结论与现象

右边有一点点多出来是因为板子右边往后弯了些。思路大概为：写好一个定点函数，根据所给坐标更新步进电机的当前对应角度。然后在直线与圆弧插补里面不断调用定点函数来进行插补。如果使用PWM脉冲中断去控制电机步进。开环控制改为闭环控制，如带编码器的伺服电机，精度也许会更高。

附下工程：https://github.com/YAOSIYANAD/Light_spot_motion_control