悬挂运动控制系统 源代码 （05年电赛）

2019-04-13 15:42发布

站内文章 / 模拟电子
9295 0 1182

题目连接：05年电赛 悬挂运动控制系统 源代码 http://wenku.baidu.com/view/b51e3a25ccbff121dd3683bf.html

系统： 2块STM32   A为上位机 液晶显示 语言提示 摄像头 + NRF 发送目标路径，  所有步进电机等由STM32 B控制。
下位机 main.c
#include "main.h" #include "math.h" #include void 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 ", 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; }

热门文章