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);
|
||
}
|
||
|
||
*/
|
||
|
||
|
||
}
|