题目连接：05年电赛 悬挂运动控制系统 源代码
系统： 2块STM32 A为上位机 液晶显示 语言提示 摄像头 + NRF 发送目标路径， 所有步进电机等由STM32 B控制。
下位机 main.c
#include "main.h"#include "math.h"#includevoid DrawCircle(){ int x , y; Move2XY(15, 50); for(x = 15; x <= 65; ++x) { y = sqrt( 625 - (x - 40)*(x - 40) ) + 50; Move2XY(x, y); } for(x = 65; x >= 15; --x) { y = 50 - sqrt( 625 - (x - 40)*(x - 40) ); Move2XY(x, y); }}void DrawL(){ int i; for(i = 0; i < 100; ++i) { Move2XY(0, i); } for(i = 0; i < 80; ++ i) { Move2XY(i,99); }}void DrawPoly(){ int i; for(i = 0; i < 100; ++i) { Move2XY(0, i); } for(i = 0; i < 80; ++ i) { Move2XY(i,99); } for(i = 0; i < 100; ++i) { Move2XY(79, 100 - i); } for(i = 0; i < 80; ++ i) { Move2XY(80 - i,0); }}int main(void){ int i; SystemInit(); GPIO_INIT(); COM1Init(115200); DrawPoly(); //DrawL(); // while(1) { DrawCircle(); }}
控制引擎
#include "Contral.h"#include "delay.h"#include "math.h"#include "usart.h"#include "stdio.h"double LeftArm = 115.974, RightArm = 149.164;int NowX = 0, NowY = 0;int StepTim = 10;int STEP[4] = {0x0011, 0x0014, 0x0044, 0x0041};int LSTEPID = 0, RSTEPID = 0;void GPIO_INIT(void){ GPIO_InitTypeDef GPIO_InitStructure; RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD , ENABLE); RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA , ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_2 | GPIO_Pin_4 | GPIO_Pin_6; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; GPIO_Init(GPIOD, &GPIO_InitStructure); GPIO_Init(GPIOA, &GPIO_InitStructure);}void LeftStepRun(int CTR, int Delay1_4){ int i = 0; if(CTR > 0) { CTR += LSTEPID; for(i = LSTEPID; i < CTR; ++i, ++LSTEPID) { GPIO_Write(GPIOD, STEP[i % 4]); delay_ms(Delay1_4); } if(LSTEPID > 10000) LSTEPID = 0; } //back if(CTR < 0) { CTR = -CTR + LSTEPID; for(i = LSTEPID; i < CTR; ++i, ++LSTEPID) { GPIO_Write(GPIOD, STEP[(4 - (i % 4)) % 4]); delay_ms(Delay1_4); } if(LSTEPID > 10000) LSTEPID = 0; } }void RightStepRun(int CTR, int Delay1_4){ int i = 0; //foward if(CTR > 0) { CTR += RSTEPID; for(i = RSTEPID; i < CTR; ++i, ++RSTEPID) { GPIO_Write(GPIOA, STEP[i % 4]); delay_ms(Delay1_4); } if(RSTEPID > 10000) RSTEPID = 0; } //back if(CTR < 0) { CTR = -CTR + RSTEPID; for(i = RSTEPID; i < CTR; ++i, ++RSTEPID) { GPIO_Write(GPIOA, STEP[(4 - (i % 4)) % 4]); delay_ms(Delay1_4); } if(RSTEPID > 10000) RSTEPID = 0; } }void Step2XY(int x, int y){ int i, j; int signL = 1, signR = 1; double tempLeftArm = 0, tempRightArm = 0; double LStepCtr = 0, RStepCtr = 0; tempLeftArm = sqrt((x + 15)*(x + 15) + (115 - y)*(115 - y)); LStepCtr = (tempLeftArm - LeftArm) / 0.05813; tempRightArm = sqrt((95 - x)*(95 - x) + (115 - y)*(115 - y)); RStepCtr = (tempRightArm - RightArm) / 0.05813; // printf("LStepCtr = %lf RStepCtr = %lf !!",LStepCtr,RStepCtr );// LeftStepRun(LStepCtr, 10);// RightStepRun(RStepCtr, 10);// printf("x = %d,, y = %d,,LStepCtr = %lf RStepCtr = %lf \n", x, y, LStepCtr, RStepCtr); i = 0; j = 0; if(LStepCtr < 0) LStepCtr = -LStepCtr, signL = -1; if(RStepCtr < 0) RStepCtr = -RStepCtr, signR = -1; while(i < LStepCtr || j < RStepCtr) { if(i < LStepCtr) LeftStepRun(signL, StepTim); if(j < RStepCtr) RightStepRun(signR, StepTim); i++; j++; } i = LStepCtr; j = RStepCtr; if(j != 0)RightArm = tempRightArm; if(i != 0)LeftArm = tempLeftArm;}int _abs(int a){ if(a > 0) return a; else return -a;}int _max(int a, int b){ if(a > b) return a; else return b;}void Move2XY(int x, int y){ int tempx, tempy, i; int xError = x - NowX, yError = y - NowY; int MaxError = _max(_abs(xError), _abs(yError)); double xSon = xError*1.0 / MaxError, ySon = yError*1.0 / MaxError; for(i = 0; i <= MaxError; ++i) { tempx = NowX + xSon * i; tempy = NowY + ySon * i; Step2XY(tempx, tempy); } NowX = x, NowY = y;}