/*
* Copyright (c) 2021, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "ti_msp_dl_config.h"
#include "main.h"
#include "stdio.h"
#include "board.h"
#include "bsp_gyro.h"
#include "motor.h"
#include "Uart.h"
#include "No_Mcu_Ganv_Grayscale_Sensor_Config.h"
#include "bsp_sr04.h"
#include "math.h" // 用于fabs()函数
#include <stdlib.h>
#define ZX_SUDU 50
extern volatile Encoder Encoder_A;
extern volatile Encoder Encoder_B;
extern short V_A,V_B;
extern float distance ;
extern uint8_t SR04_Flag ;
unsigned short Anolog[8]={0};
unsigned short white[8]={3034,2157,3303,3278,3040,3243,3239,2340};//原1524,3247,3081,3236,3205,3102,2682,2927，现3139-3219-3104-3177-3184-3109-2832-2997.........再3085-3140-3067-3128-3096-3068-2984-2999.........3034-2157-3303-3278-3040-3243-3239-2340
unsigned short black[8]={497,136,762,577,337,711,500,278};//920-923-408-1184-785-349-111-323....原761,995,508,1102,675,502,244,323.....纯黑：666-806-238-542-356-208-138-185//1938-1365-738-1230-914-766-451-797//497-136-762-577-337-711-500-278
unsigned short Normal[8];
unsigned short d[8],w=0,c=0,c1=0,zt=0,zt1=0,zt2=0,zt3=0;
short j=0;
short b[4];
float weight[4]={0.4,0.3,0.2,0.1};
float a[8], max_abs;
unsigned char rx_buff[256]={0};
/********************************************No_Mcu_Demo*******************************************/
/*****************芯片型号 MSPM0G3507 主频32Mhz ***************************************************/
/*****************引脚 AD0:PB17 AD1:PB18 AD2:PB19 ***************************************************/
/*****************OUT PA27*************************************************************************/
/********************************************No_Mcu_Demo*******************************************/
int16_t rxbuf = 0,rxbuf1=0,crxbuf=0, crxbuf1=0,cx0=0,cx = 0,cx1=0,cx2=0,cx3=0,dl=0,dr=0;
int16_t baseSpeed = ZX_SUDU;
int16_t SpeedL = 0, SpeedR = 0;
float KP = 0.5;
uint8_t oled_buffer[32];
int aim;
int V1,V2;
int V_L;
int V_R;
int output=0,output1,output2;
char PWM_Value0[10],PWM_Value1[10],PWM_Value2[10],PWM_Value3[10],PWM_Value4[10],PWM_Value5[10],PWM_Value6[10];
float SetVoltage0; //定义设定值
int js=0;
//初始化
No_MCU_Sensor sensor;
unsigned char Digtal;
//串口0发送单个字符
void uartxc_send_char(char ch);
//串口0发送字符串
void uartxc_send_string(char* str);
void Motor_Ctrl(void);
//OLED显示各项参数
void SyncParamsToOLED(void);
//获取传感器数值并处理
void SampleSensorData(void);
//等待读取病房号
void AwaitWardID(void);
//判断是否需要自选停车点
void CheckAutoParkingNeed(void);
//正常循迹
void FollowLine(void);
//遇弯道强制直行
void DisableCurveTurning(void);
//启程遇弯道转弯并记住路线，到病房开始掉头
void FollowPathCurves(void);
//返程
void ExecuteReturn(void);
//结束转弯恢复循迹
void ResumeLineFollowing(void);
int main(void)
{
SYSCFG_DL_init();
// SysTick_Init();
//清除串口中断标志
NVIC_ClearPendingIRQ(UART_K230_INST_INT_IRQN);
//使能串口中断
NVIC_EnableIRQ(UART_K230_INST_INT_IRQN);
//清除串口中断标志
NVIC_ClearPendingIRQ(UART_XC2_INST_INT_IRQN);
//使能串口中断
NVIC_EnableIRQ(UART_XC2_INST_INT_IRQN);
NVIC_ClearPendingIRQ(TIMER_2_INST_INT_IRQN);
NVIC_EnableIRQ(TIMER_2_INST_INT_IRQN);
// NVIC_ClearPendingIRQ(GPIOB_INT_IRQn);
// NVIC_EnableIRQ(GPIOB_INT_IRQn);//开启按键引脚的GPIOB端口中断
uint8_t t=' ';
Motor_Init();
OLED_Init();
// SR04_Init();
// jy61pInit();
// // lc_printf("\r\nJY61P 3D Attitude Measurement Sensor Start...\r\n");
//......................................................................感为灰度模块初始化
//初始化传感器，不带黑白值
No_MCU_Ganv_Sensor_Init_Frist(&sensor);
No_Mcu_Ganv_Sensor_Task_Without_tick(&sensor);
Get_Anolog_Value(&sensor,Anolog);
//此时打印的ADC的值，可用通过这个ADC作为黑白值的校准
//也可以自己写按键逻辑完成一键校准功能
sprintf((char *)rx_buff,"Anolog %d-%d-%d-%d-%d-%d-%d-%d\r\n",Anolog[0],Anolog[1],Anolog[2],Anolog[3],Anolog[4],Anolog[5],Anolog[6],Anolog[7]);
uart0_send_string((char *)rx_buff);
// uart0_send_string((char *)rx_buff);
// delay_cycles(CPUCLK_FREQ/10);
memset(rx_buff,0,256);
//得到黑白校准值之后，初始化传感器
No_MCU_Ganv_Sensor_Init(&sensor,white,black);
// delay_cycles(CPUCLK_FREQ/10);
while (1)
{
// Motor_On();
// DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, 0, DL_TIMER_CC_0_INDEX);
// DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST, 0, DL_TIMER_CC_1_INDEX);
// uartxc_send_string("!2@");
// delay_cycles(CPUCLK_FREQ);
//OLED显示各项参数
SyncParamsToOLED();
//获取传感器数值并处理
SampleSensorData();
//等待读取病房号
AwaitWardID();
//判断是否需要自选停车点
CheckAutoParkingNeed();
//正常循迹
FollowLine();
//遇弯道强制直行
DisableCurveTurning();
//启程遇弯道转弯并记住路线，到病房开始掉头
FollowPathCurves();
//返程
ExecuteReturn();
//结束转弯恢复循迹
ResumeLineFollowing();
// //.......................................................................JY901模块
// Gyro_Struct *JY61P_Data = get_angle();
// lc_printf("\n");
// lc_printf("JY61P RollX = [ %d ]\r\n",(int)JY61P_Data->x);
// lc_printf("JY61P PitchY = [ %d ]\r\n",(int)JY61P_Data->y);
// lc_printf("JY61P YawZ = [ %d ]\r\n",(int)JY61P_Data->z);
// delay_ms(100);
// // ..........................................................................................灰度模拟量与模拟量归一化处理应用
// sprintf((char *)rx_buff,"Digtal %d-%d-%d-%d-%d-%d-%d-%d\r\n",d[0],d[1],d[2],d[3],d[4],d[5],d[6],d[7]);
// uart0_send_string((char *)rx_buff);
// memset(rx_buff,0,256);
// //获取传感器模拟量结果(有黑白值初始化后返回1 没有返回 0)
// if(Get_Anolog_Value(&sensor,Anolog)){
// sprintf((char *)rx_buff,"Anolog %d-%d-%d-%d-%d-%d-%d-%d\r\n",Anolog[0],Anolog[1],Anolog[2],Anolog[3],Anolog[4],Anolog[5],Anolog[6],Anolog[7]);
// uart0_send_string((char *)rx_buff);
// memset(rx_buff,0,256);
// }
// //获取传感器归一化结果(只有当有黑白值传入进去了之后才会有这个值！！有黑白值初始化后返回1 没有返回 0)
// if(Get_Normalize_For_User(&sensor,Normal)){
// sprintf((char *)rx_buff,"Normalize %d-%d-%d-%d-%d-%d-%d-%d\r\n",Normal[0],Normal[1],Normal[2],Normal[3],Normal[4],Normal[5],Normal[6],Normal[7]);
// uart0_send_string((char *)rx_buff);
// memset(rx_buff,0,256);
// }
//经典版理论性能1khz，只需要delay1ms，青春版100hz，需要delay10ms，否则不能正常使用
// delay_cycles(CPUCLK_FREQ/10);
// // //........................................................................................................SR04模块
// // uint32_t Value = (int)SR04_GetLength();
// // lc_printf("Distance = %dCM\r\n", Value);
// // delay_ms(500);
// ...................................................使用串口调试参数
// sprintf((char *)rx_buff,"cx2 %d\r\n",cx2);
// uart0_send_string((char *)rx_buff);
// memset(rx_buff,0,256);
// sprintf((char *)rx_buff,"cx3 %d\r\n",cx3);
// uart0_send_string((char *)rx_buff);
// memset(rx_buff,0,256);
}
}
void GROUP1_IRQHandler(void)//Group1的中断服务函数
{
uint32_t gpio_interrup = 0;
//读取Group1的中断寄存器并清除中断标志位
switch( DL_Interrupt_getPendingGroup(DL_INTERRUPT_GROUP_1) )
{
//检查GPIO端口中断，注意是INT_IIDX
case DL_INTERRUPT_GROUP1_IIDX_GPIOB:
//获取中断信号
gpio_interrup = DL_GPIO_getEnabledInterruptStatus(ENCODER_PORT,ENCODER_E1A_PIN|ENCODER_E1B_PIN|ENCODER_E2A_PIN|ENCODER_E2B_PIN);
// encoderA
if((gpio_interrup & ENCODER_E1A_PIN)==ENCODER_E1A_PIN)
{
if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E1B_PIN))
{
Encoder_A.Should_Get_Encoder_Count--;
}
else
{
Encoder_A.Should_Get_Encoder_Count++;
}
}
else if((gpio_interrup & ENCODER_E1B_PIN)==ENCODER_E1B_PIN)
{
if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E1A_PIN))
{
Encoder_A.Should_Get_Encoder_Count++;
}
else
{
Encoder_A.Should_Get_Encoder_Count--;
}
}
// encoderB
if((gpio_interrup & ENCODER_E2A_PIN)==ENCODER_E2A_PIN)
{
if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E2B_PIN))
{
Encoder_B.Should_Get_Encoder_Count--;
}
else
{
Encoder_B.Should_Get_Encoder_Count++;
}
}
else if((gpio_interrup & ENCODER_E2B_PIN)==ENCODER_E2B_PIN)
{
if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E2A_PIN))
{
Encoder_B.Should_Get_Encoder_Count++;
}
else
{
Encoder_B.Should_Get_Encoder_Count--;
}
}
DL_GPIO_clearInterruptStatus(ENCODER_PORT,ENCODER_E1A_PIN|ENCODER_E1B_PIN|ENCODER_E2A_PIN|ENCODER_E2B_PIN);
break;
// //检查GPIO端口中断，注意是INT_IIDX
// case SR04_INT_IIDX:
// if( SR04_ECHO() ) // 上升沿
// {
// SR04_Flag = 0;
// distance = 0.0;
// Open_Timer(); //打开定时器
// }
// else // 下降沿
// {
// NVIC_DisableIRQ(SR04_INT_IRQN); // 关闭按键引脚的GPIO端口中断
// Close_Timer(); // 关闭定时器
// SR04_Flag = 1;
// distance = (float)Get_TIMER_Count() / 58.0f; // 获取时间,分辨率为1us
// }
// break;
}
}
void TIMER_0_INST_IRQHandler(void)
{
//编码器速度计算
if( DL_TimerA_getPendingInterrupt(TIMER_0_INST) == DL_TIMER_IIDX_ZERO )
{
// lc_printf("[ %d ] | [ %d ]\r\n",Motor_Get_Encoder(0),Motor_Get_Encoder(1));
// Gyro_Struct *JY61P_Data = get_angle();
GetMotorPulse();
if( DL_GPIO_readPins(KEY_PORT, KEY_PIN_21_PIN) ==0 )
{
if(DL_GPIO_readPins(KEY_PORT, KEY_PIN_21_PIN) ==0)
{
if(zt1==2)zt1=0;
}
}
if(zt1==1)
{
if(js>=150)zt1=0;
}
if(zt1==3)
{
if(js>=1900)
{
zt1=0;
}
}
// if(zt1==2)
// {
// if(js>=1000)
// {
// zt1=0;
// }
// }
if(((zt1==0)||(zt1==3))&&((cx>=1)&&(cx<=8)))
{
output1 = PID_A(baseSpeed+output);
output2 = PID_B(baseSpeed-output);
left( output1 );
right( output2 );
}
// Motor_Off();
}
}
void TIMER_2_INST_IRQHandler()
{
if( DL_TimerA_getPendingInterrupt(TIMER_2_INST) == DL_TIMER_IIDX_ZERO )
{
js++;
}
}
void UART_K230_INST_IRQHandler(void)
{
uint8_t gData;
switch (DL_UART_Main_getPendingInterrupt(UART_K230_INST)) {
case DL_UART_MAIN_IIDX_RX:
gData = DL_UART_Main_receiveData(UART_K230_INST);
if(gData == '#')
rxbuf = 0;
else if(gData == '$')
{
cx = rxbuf;
cx0=cx;
}
else
rxbuf =(gData - '0');
if(gData == '&')
rxbuf1 = 0;
else if(gData == '*')
{
cx1 = rxbuf1;
}
else
rxbuf1 =(gData - '0');
break;
default:
break;
}
}
void UART_XC2_INST_IRQHandler(void)
{
uint8_t gData;
switch (DL_UART_Main_getPendingInterrupt(UART_XC2_INST)) {
case DL_UART_MAIN_IIDX_RX:
gData = DL_UART_Main_receiveData(UART_XC2_INST);
if(gData == '!')
crxbuf = 0;
else if(gData == '@')
{
cx2 = crxbuf;
}
else
crxbuf =(gData - '0');
if(gData == '%')
crxbuf1 = 0;
else if(gData == '^')
{
cx3 = crxbuf1;
}
else
crxbuf1 =(gData - '0');
break;
default:
break;
}
}
//OLED显示各项参数
void SyncParamsToOLED(void)
{
sprintf(PWM_Value0,"CX:%d",cx);
OLED_ShowString(0,0,(uint8_t *)PWM_Value0,8);
sprintf(PWM_Value1,"ZT2:%d",zt2);
OLED_ShowString(0,1,(uint8_t *)PWM_Value1,16);
sprintf(PWM_Value2,"CX1:%d",cx1);
OLED_ShowString(0,3,(uint8_t *)PWM_Value2,8);
sprintf(PWM_Value3,"ZT1:%d",zt1);
OLED_ShowString(0,4,(uint8_t *)PWM_Value3,8);
sprintf(PWM_Value4,"C:%d",c);
OLED_ShowString(0,5,(uint8_t *)PWM_Value4,8);
sprintf(PWM_Value5,"zt:%d",zt);
OLED_ShowString(0,6,(uint8_t *)PWM_Value5,8);
sprintf(PWM_Value6,"C1:%d",c1);
OLED_ShowString(0,7,(uint8_t *)PWM_Value6,8);
}
//zt：是否转弯，=3时左转，=4时右转，=0时巡线，=1时转头；zt1：=1时是否延迟转弯，=3是否转弯延时，=2时是否停车等待1秒钟；zt2：是否进入返程模式，=1为返程，=0为启程;zt3：去远端病房是否进入二次转弯；w：灰度是否全是识别到红色（转弯处），=1为是；c：灰度是否全识别到白色（药房），=0为是；c1：灰度检测到红色的数量；js：定时器计时代替延时函数使用的计时变量单位1ms。cx：送药前识别到需要去的病房号；cx1：送药中识别到的病房号；cx：需要去的病房；cx0：再次储存需要去的病房号；cx1：在路途中识别到的病房号；cx2：自选的停车点；cx3：从自选停车点到病房需要的操作指示：=1开始回头，遇线等待，且反转返程路线，=2退出等待且重置zt2为启程模式使其去到下一个病房。
//获取传感器数值并处理
void SampleSensorData(void)
{
//......................................................................感为灰度模块
//无时基传感器常规任务，包含模拟量，数字量，归一化量
No_Mcu_Ganv_Sensor_Task_Without_tick(&sensor);
//获取传感器数字量结果(只有当有黑白值传入进去了之后才会有这个值！！)
Digtal=Get_Digtal_For_User(&sensor);
// sprintf((char *)rx_buff,"Digtal %d-%d-%d-%d-%d-%d-%d-%d\r\n",(Digtal>>0)&0x01,(Digtal>>1)&0x01,(Digtal>>2)&0x01,(Digtal>>3)&0x01,(Digtal>>4)&0x01,(Digtal>>5)&0x01,(Digtal>>6)&0x01,(Digtal>>7)&0x01);
// uart0_send_string((char *)rx_buff);
// memset(rx_buff,0,256);
//处理数据
for(int i=0;i<8;i++)
{
d[i]=(Digtal>>i)&0x01;
}
w=d[0]&d[1]&d[2]&d[3]&d[4]&d[5]&d[6]&d[7];
c=d[0]|d[1]|d[2]|d[3]|d[4]|d[5]|d[6]|d[7];
c1=d[0]+d[1]+d[2]+d[3]+d[4]+d[5]+d[6]+d[7];
}
//等待读取病房号
void AwaitWardID(void)
{
while(!((cx>=1)&&(cx<=8)))
{
Motor_Off();
}
}
//判断是否需要自选停车点
void CheckAutoParkingNeed(void)
{
if(cx2!=0)
{
cx=cx2;
}
}
//正常循迹
void FollowLine(void)
{
if((zt==0)&&(zt1!=3))
{
if(d[3])output=0;
if(d[2])output=-15;
if(d[1])output=-20;
if(d[0])output=-35;
if(d[4])output=0;
if(d[5])output=15;
if(d[6])output=20;
if(d[7])output=35;
}
}
//遇弯道强制直行
/**
* @brief Disable curve turning (禁用弯道转向)
* @note Maintains current heading (保持当前航向)
*/
void DisableCurveTurning(void)
{
if(zt2==0)
{
if((cx1==0)&&(cx!=1)&&(cx!=2))
{
if(c1>=4)
output=0;
}
}
if(zt2==1)
{
if(j<=0)
{
if(!c)
{
zt1=2;
// js=0;
uartxc_send_string("%2^");
while(zt1==2)
{
Motor_Off();
}
}
if(c1>=4)
{
output=0;
}
}
}
}
//启程遇弯道转弯并记住路线，到病房开始掉头
void FollowPathCurves(void)
{
if(zt2==0)
{
if(zt==0)
{
if(c1>=5)
{
if(cx==1)
{
zt1=1;
js=0;
output=-20;
baseSpeed=0;
zt=3;
}
if(cx==2)
{
zt1=1;
js=0;
output=20;
baseSpeed=0;
zt=4;
}
if(cx==cx1)
{
if((cx==8)&&(zt3==1))
{
cx=7;
}
else if((cx==5)&&(zt3==1))
{
cx=6;
}
if(cx%2==0)
{
zt1=1;
js=0;
output=20;
baseSpeed=0;
zt=4;
zt3=1;
}
if(cx%2!=0)
{
zt1=1;
js=0;
output=-20;
baseSpeed=0;
zt=3;
zt3=1;
}
}
}
if(!c)
{
if(cx2!=0)
{
DL_GPIO_setPins(GPIOB, DL_GPIO_PIN_22);
cx2=0;
cx=cx0;
}
zt=1;
zt1=2;
// js=0;
uartxc_send_string("!2@");
while(zt1==2)
{
Motor_Off();
if(cx3==1)
{
zt1=0;
DL_GPIO_clearPins(GPIOB, DL_GPIO_PIN_22);
}
}
uartxc_send_string("%1^");
output=-30;
baseSpeed=0;
zt1=3;
js=0;
}
}
}
}
//返程
/**
* @brief Execute return journey (执行返程)
* @note Follows pre-recorded path in reverse (沿预存路径反向行驶)
*/
void ExecuteReturn(void)
{
if((zt2==1)&&(zt1!=3))
{
if(c1>=4)
{
if(j>0)
{
output=b[j-1]*20;
baseSpeed=0;
if(output>0)
{
zt=4;
zt1=1;
js=0;
}
if(output<0)
{
zt=3;
zt1=1;
js=0;
}
}
}
}
}
//结束转弯恢复循迹
/**
* @brief Resume line following (恢复正常循迹)
* @note Switches from turn mode to straight tracking (从转弯模式切换为直线跟踪)
*/
void ResumeLineFollowing(void)
{
if(zt==3)
{
if(((d[0]==1))&&(c1==1))
{
zt=0;
baseSpeed=ZX_SUDU;
if(zt2==0)
{
b[j]=1;
j++;
}
if(zt2==1)
{
j--;
}
if(cx3==2)
{
zt2=0;
cx3=0;
}
}
}
else if(zt==4)
{
if(((d[7]==1))&&(c1==1))
{
zt=0;
baseSpeed=ZX_SUDU;
if(zt2==0)
{
b[j]=-1;
j++;
}
if(zt2==1)
{
j--;
}
if(cx3==2)
{
zt2=0;
cx3=0;
}
}
}
else if(zt==1)
{
if((c)&&(c1==1)&&(zt1!=3))
{
if(cx3==1)
{
b[j-1]=-b[j-1];
}
while(cx3==1)
{
Motor_Off();
}
baseSpeed=ZX_SUDU;
zt=0;
zt2=1;
}
}
}
//串口发送单个字符
void uartxc_send_char(char ch)
{
//当串口0忙的时候等待，不忙的时候再发送传进来的字符
while( DL_UART_isBusy(UART_XC2_INST) == true );
//发送单个字符
DL_UART_Main_transmitData(UART_XC2_INST, ch);
}
//串口发送字符串
void uartxc_send_string(char* str)
{
//当前字符串地址不在结尾 并且 字符串首地址不为空
while(*str!=0&&str!=0)
{
//发送字符串首地址中的字符，并且在发送完成之后首地址自增
uartxc_send_char(*str++);
}
}
// void Motor_Ctrl(void)
// {
// SpeedL = baseSpeed + cx*KP;//.....................................改
// SpeedR = baseSpeed - cx*KP;//
// if(SpeedL > 40)
// SpeedL = 40;
// else if(SpeedL < 0)
// SpeedL = 0;
// if(SpeedR > 40)
// SpeedR = 40;
// else if(SpeedR < 0)
// SpeedR = 0;
// Set_Speed(0, SpeedL);
// Set_Speed(1, SpeedR);
// }
分析代码
扫一扫
3913
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
