282 lines
4.4 KiB
C
282 lines
4.4 KiB
C
|
#include<stm32f10x.h>
|
|||
|
|
|||
|
//延时用于指示灯闪烁
|
|||
|
void delay(int k)
|
|||
|
{
|
|||
|
while(k--)
|
|||
|
{
|
|||
|
int i=1000;
|
|||
|
while(i--)
|
|||
|
{
|
|||
|
int j=100;
|
|||
|
while(j--);
|
|||
|
}
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
|
|||
|
//各舵机复位
|
|||
|
void ServoInit()
|
|||
|
{
|
|||
|
TIM3->CCR2=2100;
|
|||
|
delay(20);
|
|||
|
TIM2->CCR4=1000;
|
|||
|
delay(20);
|
|||
|
TIM2->CCR3=2300;
|
|||
|
delay(20);
|
|||
|
TIM2->CCR2=850;
|
|||
|
delay(20);
|
|||
|
TIM2->CCR1=1400;
|
|||
|
delay(50); //各舵机复位
|
|||
|
}
|
|||
|
|
|||
|
//初始化TIM2的四个通道为PWM模式1
|
|||
|
void TIM2_Init()
|
|||
|
{
|
|||
|
RCC->APB1ENR|=3; //TIM2,TIM3时钟使能
|
|||
|
RCC->APB2ENR|=1<<2; //GPIOA时钟使能
|
|||
|
|
|||
|
GPIOA->CRL=0xB333BBBB; //GPIOA.0-3,7复用推挽输出,GPIOA.4-6通用推挽输出
|
|||
|
|
|||
|
TIM2->PSC=71;
|
|||
|
TIM3->PSC=71; //CNT频率1MHz
|
|||
|
TIM2->ARR=20000;
|
|||
|
TIM3->ARR=20000; //PWM周期20ms
|
|||
|
|
|||
|
TIM2->CCMR1|=6<<4; //CH1配置PWM1模式
|
|||
|
TIM2->CCMR1|=1<<3; //CH1自动重装载预装载使能
|
|||
|
TIM2->CCER|=1; //CH1比较使能
|
|||
|
TIM2->CCMR1|=6<<12; //CH2配置PWM1模式
|
|||
|
TIM2->CCMR1|=1<<11; //CH2自动重装载预装载使能
|
|||
|
TIM2->CCER|=1<<4; //CH2比较使能
|
|||
|
TIM2->CCMR2|=6<<4; //CH3配置PWM1模式
|
|||
|
TIM2->CCMR2|=1<<3; //CH3自动重装载预装载使能
|
|||
|
TIM2->CCER|=1<<8; //CH3比较使能
|
|||
|
TIM2->CCMR2|=6<<12; //CH4配置PWM1模式
|
|||
|
TIM2->CCMR2|=1<<11; //CH4自动重装载预装载使能
|
|||
|
TIM2->CCER|=1<<12; //CH4比较使能
|
|||
|
TIM3->CCMR1|=6<<12; //CH2配置PWM1模式
|
|||
|
TIM3->CCMR1|=1<<11; //CH2自动重装载预装载使能
|
|||
|
TIM3->CCER|=1<<4; //CH2比较使能
|
|||
|
|
|||
|
TIM2->CR1|=1<<7; //TIM2自动重装载预装载使能
|
|||
|
TIM3->CR1|=1<<7; //TIM3自动重装载预装载使能
|
|||
|
TIM2->CR1|=1; //TIM2使能
|
|||
|
TIM3->CR1|=1; //TIM3使能
|
|||
|
|
|||
|
// -90 530
|
|||
|
// 0 1400
|
|||
|
// +90 2400
|
|||
|
//大约100对应10度
|
|||
|
ServoInit();
|
|||
|
|
|||
|
}
|
|||
|
|
|||
|
//颜色识别模块初始化代码
|
|||
|
void Color_Init()
|
|||
|
{
|
|||
|
GPIOA->BSRR=1<<4;
|
|||
|
GPIOA->BSRR=1<<5;
|
|||
|
GPIOA->BSRR=1<<6;
|
|||
|
delay(50);
|
|||
|
GPIOA->BRR=1<<4;
|
|||
|
GPIOA->BRR=1<<5;
|
|||
|
GPIOA->BRR=1<<6;
|
|||
|
delay(20);
|
|||
|
}
|
|||
|
|
|||
|
//初始化GPIOC用于工作指示灯
|
|||
|
void GPIOC_Init()
|
|||
|
{
|
|||
|
RCC->APB2ENR|=1<<4; //GPIOC时钟使能
|
|||
|
GPIOC->CRH=0;
|
|||
|
GPIOC->CRH|=3<<20; //配置PC.13口为通用推挽输出
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
//抓手释放
|
|||
|
void release()
|
|||
|
{
|
|||
|
TIM3->CCR2=1400;
|
|||
|
delay(10);
|
|||
|
}
|
|||
|
|
|||
|
//抓手夹持
|
|||
|
void hold()
|
|||
|
{
|
|||
|
TIM3->CCR2=2100;
|
|||
|
delay(10);
|
|||
|
}
|
|||
|
|
|||
|
void moveServo1(int i)
|
|||
|
{
|
|||
|
if(i>2400||i<530) return;
|
|||
|
while(TIM2->CCR1>i)
|
|||
|
{
|
|||
|
TIM2->CCR1-=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
while(TIM2->CCR1<i)
|
|||
|
{
|
|||
|
TIM2->CCR1+=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
void moveServo2(int i)
|
|||
|
{
|
|||
|
if(i>2400||i<530) return;
|
|||
|
while(TIM2->CCR2>i)
|
|||
|
{
|
|||
|
TIM2->CCR2-=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
while(TIM2->CCR2<i)
|
|||
|
{
|
|||
|
TIM2->CCR2+=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
void moveServo3(int i)
|
|||
|
{
|
|||
|
if(i>2400||i<530) return;
|
|||
|
while(TIM2->CCR3>i)
|
|||
|
{
|
|||
|
TIM2->CCR3-=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
while(TIM2->CCR3<i)
|
|||
|
{
|
|||
|
TIM2->CCR3+=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void moveServo4(int i)
|
|||
|
{
|
|||
|
if(i>2400||i<530) return;
|
|||
|
while(TIM2->CCR4>i)
|
|||
|
{
|
|||
|
TIM2->CCR4-=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
while(TIM2->CCR4<i)
|
|||
|
{
|
|||
|
TIM2->CCR4+=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void catch30()
|
|||
|
{
|
|||
|
moveServo4(1200);
|
|||
|
moveServo3(2000);
|
|||
|
moveServo1(820);
|
|||
|
release();
|
|||
|
moveServo2(1570);
|
|||
|
hold();
|
|||
|
moveServo2(850);
|
|||
|
moveServo1(2200);
|
|||
|
moveServo4(1300);
|
|||
|
moveServo3(2300);
|
|||
|
moveServo2(1400);
|
|||
|
release();
|
|||
|
delay(20);
|
|||
|
moveServo1(1950);
|
|||
|
delay(10);
|
|||
|
moveServo2(850);
|
|||
|
ServoInit();
|
|||
|
}
|
|||
|
|
|||
|
void catch60()
|
|||
|
{
|
|||
|
moveServo4(1000);
|
|||
|
moveServo3(1600);
|
|||
|
moveServo1(1100);
|
|||
|
release();
|
|||
|
moveServo2(1770);
|
|||
|
hold();
|
|||
|
moveServo2(850);
|
|||
|
moveServo1(2270);
|
|||
|
moveServo4(1250);
|
|||
|
moveServo3(2100);
|
|||
|
moveServo2(1450);
|
|||
|
release();
|
|||
|
delay(20);
|
|||
|
moveServo1(2100);
|
|||
|
delay(10);
|
|||
|
moveServo2(850);
|
|||
|
ServoInit();
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
void catch135()
|
|||
|
{
|
|||
|
moveServo4(1200);
|
|||
|
moveServo3(2000);
|
|||
|
moveServo1(1900);
|
|||
|
release();
|
|||
|
moveServo2(1570);
|
|||
|
hold();
|
|||
|
moveServo2(850);
|
|||
|
moveServo1(2350);
|
|||
|
moveServo4(900);
|
|||
|
moveServo3(1600);
|
|||
|
moveServo2(1700);
|
|||
|
release();
|
|||
|
delay(20);
|
|||
|
moveServo1(2100);
|
|||
|
delay(10);
|
|||
|
moveServo2(850);
|
|||
|
ServoInit();
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
int main()
|
|||
|
{
|
|||
|
TIM2_Init();
|
|||
|
GPIOC_Init();
|
|||
|
Color_Init();
|
|||
|
catch30();
|
|||
|
catch60();
|
|||
|
catch135();
|
|||
|
|
|||
|
while(1)
|
|||
|
{
|
|||
|
GPIOC->BSRR=1<<13; //开PC.13
|
|||
|
delay(5);
|
|||
|
GPIOC->BRR=1<<13; //关PC.13
|
|||
|
delay(3);
|
|||
|
}
|
|||
|
|
|||
|
|
|||
|
/*
|
|||
|
while(1)
|
|||
|
{
|
|||
|
while(TIM2->CCR4<1100)
|
|||
|
{
|
|||
|
TIM2->CCR4+=20;
|
|||
|
TIM2->CCR3+=20;
|
|||
|
TIM2->CCR2+=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
delay(3);
|
|||
|
while(TIM2->CCR4>530)
|
|||
|
{
|
|||
|
TIM2->CCR4-=20;
|
|||
|
TIM2->CCR3-=20;
|
|||
|
TIM2->CCR2-=10;
|
|||
|
delay(1);
|
|||
|
}
|
|||
|
delay(3);
|
|||
|
}
|
|||
|
|
|||
|
*/
|
|||
|
|
|||
|
|
|||
|
}
|