悬挂运动控制系统 源代码 （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 \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;

}