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

题目连接：05年电赛 悬挂运动控制系统 源代码

http://wenku.baidu.com/view/b51e3a25ccbff121dd3683bf.html

系统： 2块STM32   A为上位机 液晶显示 语言提示 摄像头 + NRF 发送目标路径，  所有步进电机等由STM32 B控制。

下位机 main.c

#include "main.h"
#include "math.h"
#include <stdio.h>

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;
}