基于orangepi zero的智能小车 -- 结项
时间:10-02
整理:3721RD
点击:
小弟我也是第一次在电子发烧友上申请到板子使用,十分感激发烧友为我提供的orangepi zero开发板,希望能够做一次好的评测,但是由于小弟我水平有限,加上年前各种事情很忙很忙,可能我的评测并没有大家期望的结果,不过还是希望我的评测能够帮助到论坛上的学友们,希望大家不吝赐教。
首先我是第一次用嵌入式Linux开发板进行硬件开发,之前做这些小车之类的“玩具”都是使用STM32、51单片机之类的处理器,虽然STM32也能跑各种嵌入式实时操作系统,但是和Linux的编程方式还是有很大的不同,在Linux系统下对于GPIO的操作都会复杂很多,但是Linux也提供了更多的功能,这也不是STM32之类的处理器能够比拟的。
开发初期的话是对开发环境和开发过程的熟悉,包括开发板的系统安装,编辑器VIM的使用,系统软件的安装,NFS服务器的搭建,随后进行GPIO控制、IIC总线测试、串口读取测试、车模电机驱动、电机编码盘驱动编写,最后到进行车模进行行走的程序设计。
前面的帖子总会如下:
http://bbs.elecfans.com/forum.ph ... &tid=1100804&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1101528&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1101718&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1102155&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1102353&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1102458&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1102824&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1104313&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1105396&extra=
http://bbs.elecfans.com/forum.ph ... &tid=1106766&extra=
我为车子加了6轴姿态传感器、4个红外感应传感器、两个编码盘、一个电机启动模块、GPS模块(由于一直在室内进行测试,无法接受到GPS信号,所以GPS模块没有使用),车子整体如下图:
四个红外传感器两个用于检测前方有没有障碍物,用于自动避障,两个用于检测地面黑线,用于寻迹。整个系统使用一个4000mah的充电宝作为电源供电,可以长时间运行。车子有三种工作模式,第一种是人工遥控控制小车运行,第二种是小车沿着黑线的位置进行寻迹,最后一种是自动避障运行,下面是三种运行模式的视频链接,我是在租的小房间里面实验的,没法找到大场地。基于orangepi zero的智能小车 -- 手动遥控:
基于orangepi zero的智能小车 -- 寻迹:
基于orangepi zero的智能小车 -- 自动避障:
下面是小车的主程序:
- #include <stdio.h>
- #include <stdlib.h>
- #include <fcntl.h>
- #include <linux/i2c-dev.h>
- #include <errno.h>
- #include "gpio.h"
- #include "i2c.h"
- #include "math.h"
- #include "pthread.h"
- #include <termios.h>
- #include <sys/timeb.h>
- #include <signal.h> //signal()
- #include <string.h> //memset()
- #include <sys/time.h> //struct itimerval, setitimer()
- #include <semaphore.h>
- #include "./DMP/inv_mpu.h"
- #include "./DMP/inv_mpu_dmp_motion_driver.h"
- #define RIGHT_WHEEL 0
- #define LEFT_WHEEL 1
- typedef enum
- {
- false = 0,
- true = 1,
- }bool;
- static signed char gyro_orientation[9] = {-1, 0, 0,
- 0,-1, 0,
- 0, 0, 1};
- //fsr即Full Scale Range量程
- #define q30 1073741824.0f
- float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
- char num[50];
- float Pitch,Roll,Yaw;
- unsigned long sensor_timestamp;
- short gyro[3], accel[3], sensors;
- unsigned char more;
- long quat[4];
- char buf[256];
- unsigned char CoderSpeed[2];
- int coder_fd;
- int PWMLeft = 0,PWMRight = 0;
- bool IsAutoDrive = false;
- bool IsRailDrive = false;
- sem_t Sem_IIC;
- void p_interaction(void);
- void p_mpu6050reader(int signo);
- void DMP_Init(void)
- {
- int i,result;
- sem_wait(&Sem_IIC); //申请信号量
-
- IIC_SetAddr(0x68);
-
- result = mpu_init();
- if(!result)
- {
- printf("mpu initialization complete......\n\r\n");
- //mpu_set_sensor 设置传感器
- if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
- printf("mpu_set_sensor complete ......\n\r\n");
- else
- printf("mpu_set_sensor come across error ......\n\r\n");
- //mpu_configure_fifo 配置FIFO
- if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
- printf("mpu_configure_fifo complete ......\n\r\n");
- else
- printf("mpu_configure_fifo come across error ......\n\r\n");
- //mpu_set_sample_rate-----------------设置数据采样速率,默认是DEFAULT_MPU_HZ频率为100hz,在4hz到1000hz之间
- if(!mpu_set_sample_rate(iMPU_HZ))
- printf("mpu_set_sample_rate complete ......\n\r\n");
- else
- printf("mpu_set_sample_rate error ......\n\r\n");
- //dmp_load_motion_driver_firmvare-----加载运动引擎固件
- if(!dmp_load_motion_driver_firmware())
- printf("dmp_load_motion_driver_firmware complete ......\n\r\n");
- else
- printf("dmp_load_motion_driver_firmware come across error ......\n\r\n");
- //dmp_set_orientation-----------------设定方向
- if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
- printf("dmp_set_orientation complete ......\n\r\n");
- else
- printf("dmp_set_orientation come across error ......\n\r\n");
- //dmp_enable_feature------------------使能功能特性
- if(!dmp_enable_feature( DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT |
- DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL))
- printf("dmp_enable_feature complete ......\n\r\n");
- else
- printf("dmp_enable_feature come across error ......\n\r\n");
- //dmp_set_fifo_rate-------------------设置FIFO速率,默认是DEFAULT_MPU_HZ频率为100hz,采样速率不得大于 DMP_SAMPLE_RATE(200hz)
- if(!dmp_set_fifo_rate(iFIFO_HZ))
- printf("dmp_set_fifo_rate complete ......\n\r\n");
- else
- printf("dmp_set_fifo_rate come across error ......\n\r\n");
- run_self_test();
- if(!mpu_set_dmp_state(1))
- printf("mpu_set_dmp_state complete ......\n\r\n");
- else
- printf("mpu_set_dmp_state come across error ......\n\r\n");
- }
- else
- {
- printf("mpu initialization error......\n\r\n");
- exit(0);
- }
-
- sem_post(&Sem_IIC); //释放信号量
- }
- void DataProcess(void)
- {
- sem_wait(&Sem_IIC); //申请信号量
-
- IIC_SetAddr(0x68);
- /* Gyro and accel data are written to the FIFO by the DMP in chip
- * frame and hardware units. This behavior is convenient because it
- * keeps the gyro and accel outputs of dmp_read_fifo and
- * mpu_read_fifo consistent.
- */
- dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
- /* Unlike gyro and accel, quaternions are written to the FIFO in
- * the body frame, q30. The orientation is set by the scalar passed
- * to dmp_set_orientation during initialization.
- */
- if(sensors & INV_WXYZ_QUAT)
- {
- q0 = quat[0] / q30;
- q1 = quat[1] / q30;
- q2 = quat[2] / q30;
- q3 = quat[3] / q30;
- Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
- Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
- Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; // yaw没有实际用处,没有磁阻传感器会产生零飘
- }
-
- sem_post(&Sem_IIC); //释放信号量
- }
- void SetUpPCA9685()
- {
- sem_wait(&Sem_IIC); //申请信号量
- IIC_SetAddr(0x40);
- IIC_Write(0x00,0x10);
- IIC_Write(0xfd,3);
- IIC_Write(0x00,0x00);
- int i;
- for(i=6;i<=69;i++)
- {
- IIC_Write((unsigned char)i,0x00);
- }
-
- // for(i=0;i<=255;i++)
- // {
- // printf("%.2X ",IIC_Read(i));
- // if((i+1 & 0x0F) == 0)
- // printf("\r\n");
- // }
- sem_post(&Sem_IIC); //释放信号量
- }
- void PCA9685_SetPWM(int channel,unsigned short pwm)
- {
- unsigned char h,l;
- h = (unsigned char)(pwm >> 8);
- l = (unsigned char)(pwm & 0xFF);
- // printf("%d,%d\r\n",h,l);
- IIC_Write((unsigned char)(channel * 4 + 0x08),l);
- // printf("%.2X ",IIC_Read((unsigned char)(channel * 4 + 0x08)));
- IIC_Write((unsigned char)(channel * 4 + 0x08 + 1),h);
- // printf("%.2X\r\n",IIC_Read((unsigned char)(channel * 4 + 0x08 + 1)));
- }
- void SetWheel(int wheel,int pwm)
- {
- unsigned short temp = (unsigned short)((pwm > 0)?pwm:-pwm);
- sem_wait(&Sem_IIC); //申请信号量
- IIC_SetAddr(0x40);
- // printf("%d,%d\r\n",wheel,pwm);
- if(wheel == LEFT_WHEEL)
- {
- if(pwm > 0)
- {
- PCA9685_SetPWM(2,0);
- PCA9685_SetPWM(3,temp);
- }
- else
- {
- PCA9685_SetPWM(2,temp);
- PCA9685_SetPWM(3,0);
- }
- }
- else
- {
- if(pwm > 0)
- {
- PCA9685_SetPWM(0,temp);
- PCA9685_SetPWM(1,0);
- }
- else
- {
- PCA9685_SetPWM(0,0);
- PCA9685_SetPWM(1,temp);
- }
- }
-
- sem_post(&Sem_IIC); //释放信号量
- }
- void ReadCoder()
- {
- read(coder_fd,CoderSpeed,2);
- printf("%d,%d\r\n",CoderSpeed[LEFT_WHEEL],CoderSpeed[RIGHT_WHEEL]);
- }
- void TurnRight_Angle(int ang)
- {
- int pre,dst,temp;
- pre = (int)Yaw;
- dst = pre - ang;
- printf("pre:%d,dst:%d\r\n",pre,dst);
-
- SetWheel(LEFT_WHEEL,3000);
- SetWheel(RIGHT_WHEEL,-3000);
-
- if(dst < -180)
- {
- printf("dst < -180\r\n");
- temp = 360 - (-dst);
- printf("temp:%d\r\n",temp);
- while((int)Yaw < temp);
- printf("Yaw:%d\r\n",(int)Yaw);
- while((int)Yaw > temp);
- printf("Yaw:%d\r\n",(int)Yaw);
- }
- else
- {
- printf("dst >= -180\r\n");
- while((int)Yaw > dst);
- printf("%d\r\n",(int)Yaw);
- }
- SetWheel(LEFT_WHEEL,-2000);
- SetWheel(RIGHT_WHEEL,2000);
- usleep(10000);
- printf("ok\r\n\r\n");
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- }
- void TurnLeft_Angle(int ang)
- {
- int pre,dst,temp;
- pre = (int)Yaw;
- dst = pre + ang;
- printf("pre:%d,dst:%d\r\n",pre,dst);
-
- SetWheel(LEFT_WHEEL,-3000);
- SetWheel(RIGHT_WHEEL,3000);
-
- if(dst > 180)
- {
- printf("dst > 180\r\n");
- temp = dst - 360;
- printf("temp:%d\r\n",temp);
- while((int)Yaw > temp);
- printf("Yaw:%d\r\n",(int)Yaw);
- while((int)Yaw < temp);
- printf("Yaw:%d\r\n",(int)Yaw);
- }
- else
- {
- printf("dst <= 180\r\n");
- while((int)Yaw < dst);
- printf("%d\r\n",(int)Yaw);
- }
- SetWheel(LEFT_WHEEL,2000);
- SetWheel(RIGHT_WHEEL,-2000);
- usleep(10000);
- printf("ok\r\n\r\n");
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- }
- void TurnRight(int tim)
- {
- SetWheel(LEFT_WHEEL,3000);
- SetWheel(RIGHT_WHEEL,-3000);
- usleep(1000 * tim);
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- }
- void TurnLeft(int tim)
- {
- SetWheel(LEFT_WHEEL,-3000);
- SetWheel(RIGHT_WHEEL,3000);
- usleep(1000 * tim);
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- }
- void Goback(int tim)
- {
- SetWheel(LEFT_WHEEL,-3000);
- SetWheel(RIGHT_WHEEL,-3000);
- usleep(1000 * tim);
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- }
- static struct termios oldt;
- //restore terminal settings
- void restore_terminal_settings(void)
- {
- //Apply saved settings
- tcsetattr(0, TCSANOW, &oldt);
- }
- //make terminal read 1 char at a time
- void disable_terminal_return(void)
- {
- struct termios newt;
-
- //save terminal settings
- tcgetattr(0, &oldt);
- //init new settings
- newt = oldt;
- //change settings
- newt.c_lflag &= ~(ICANON | ECHO);
- //apply settings
- tcsetattr(0, TCSANOW, &newt);
-
- //make sure settings will be restored when program ends
- atexit(restore_terminal_settings);
- }
- int main(void)
- {
- disable_terminal_return();
- GPIO_Init();
- if((coder_fd = open("/dev/zzz",O_RDWR)) < 0)
- return;
- // read(coder_fd,CoderSpeed,8);
- // printf("%d,%d\r\n",CoderSpeed[LEFT_WHEEL],CoderSpeed[RIGHT_WHEEL]);
-
- GPIO_ConfigPin(PA,10,IN);
- GPIO_ConfigPin(PA,13,IN);
-
- GPIO_ConfigPin(PA,18,IN);
- GPIO_ConfigPin(PA,19,IN);
-
- sem_init(&Sem_IIC, 0, 1);
-
- //MPU6050的初始化工作必须在上点之后的几百毫秒之后,不然读出来的数据都是0,保险起见延时1s后初始化
- //并且在每条初始化语句后面都要加上适当延时,不然还是会有数据不正常情况的发生.
- usleep(1000000);
- int result;
- result = IIC_Open();
- SetUpPCA9685();
- DMP_Init();
-
- pthread_t runner,MPU6050_Reader;
- pthread_create(&runner,NULL,(void*)&p_interaction,NULL);
- pthread_create(&MPU6050_Reader,NULL,(void*)&p_mpu6050reader,NULL);
-
- int a,b,c,d;
- while(1)
- {
- if(IsAutoDrive)
- {
- printf("Auto Drive\r\n");
- while(1)
- {
- SetWheel(LEFT_WHEEL,2000);
- SetWheel(RIGHT_WHEEL,2000);
-
- a = GPIO_GetPin(PA,10);
- b = GPIO_GetPin(PA,13);
- if(a && !b)
- {
- TurnLeft(300);
- }
- else if(!a && b)
- {
- TurnRight(300);
- }
- else if(a && b)
- {
- TurnRight(1000);
- }
-
- if(!IsAutoDrive)
- {
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- break;
- }
- usleep(10000);
- }
- }
- else if(IsRailDrive)
- {
- printf("Rail Drive\r\n");
- while(1)
- {
- a = GPIO_GetPin(PA,18);
- b = GPIO_GetPin(PA,19);
- c = GPIO_GetPin(PA,10);
- d = GPIO_GetPin(PA,13);
- if(c || d)
- {
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- }
- else
- {
- if(!a && b)
- {
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,4000);
- while(1)
- {
- a = GPIO_GetPin(PA,18);
- b = GPIO_GetPin(PA,19);
- if(!(a ^ b))
- {
- break;
- }
- }
- }
- else if(a && !b)
- {
- SetWheel(LEFT_WHEEL,4000);
- SetWheel(RIGHT_WHEEL,0);
- while(1)
- {
- a = GPIO_GetPin(PA,18);
- b = GPIO_GetPin(PA,19);
- if(!(a ^ b))
- {
- break;
- }
- }
- }
- else
- {
- SetWheel(LEFT_WHEEL,2000);
- SetWheel(RIGHT_WHEEL,2000);
- }
- }
-
- if(!IsRailDrive)
- {
- SetWheel(LEFT_WHEEL,0);
- SetWheel(RIGHT_WHEEL,0);
- break;
- }
- usleep(10000);
- }
- }
- }
- }
- void p_interaction(void)
- {
- char cmd_char;
- while(1)
- {
- cmd_char = (char)getchar();
- // printf("%c",cmd_char);
- // fflush(stdout);
-
- if(IsAutoDrive || IsRailDrive)
- {
- if(cmd_char == 'e')
- {
- IsAutoDrive = false;
- PWMLeft = 0;
- PWMRight = 0;
- }
- if(cmd_char == 'r')
- {
- IsRailDrive = false;
- PWMLeft = 0;
- PWMRight = 0;
- }
- }
- else
- {
- if(cmd_char == 'w')
- {
- PWMLeft = 3000;
- PWMRight = 3000;
- }
- else if(cmd_char == 'a')
- {
- PWMLeft = -3000;
- PWMRight = 3000;
- }
- else if(cmd_char == 's')
- {
- PWMLeft = -3000;
- PWMRight = -3000;
- }
- else if(cmd_char == 'd')
- {
- PWMLeft = 3000;
- PWMRight = -3000;
- }
- else if(cmd_char == 'q')
- {
- PWMLeft = 4000;
- PWMRight = 4000;
- }
- else if(cmd_char == ' ')
- {
- PWMLeft = 0;
- PWMRight = 0;
- }
- else if(cmd_char == 'e')
- {
- IsAutoDrive = true;
- PWMLeft = 0;
- PWMRight = 0;
- }
- else if(cmd_char == 'r')
- {
- IsRailDrive = true;
- PWMLeft = 0;
- PWMRight = 0;
- }
- SetWheel(LEFT_WHEEL,PWMLeft);
- SetWheel(RIGHT_WHEEL,PWMRight);
- }
- }
- }
- void p_mpu6050reader(int signo)
- {
- while(1)
- {
- DataProcess();
- // printf("%.4f,%.4f,%.4f\r\n",Pitch,Roll,Yaw);
- usleep(10000);
- }
- }
下面是电机编码盘的驱动程序:
- #include <linux/errno.h>
- #include <linux/kernel.h>
- #include <linux/module.h>
- #include <linux/slab.h>
- #include <linux/input.h>
- #include <linux/init.h>
- #include <linux/serio.h>
- #include <linux/delay.h>
- #include <linux/clk.h>
- #include <asm/io.h>
- #include <asm/irq.h>
- #include <asm/uaccess.h>
- #include <linux/cdev.h>
- #include <linux/miscdevice.h>
- #include <linux/timer.h>
- #define DEVICE_NAME "CODER"
- #define DEVICE_MAJOR 246 /* 主设备号*/
- #define DEVICE_MINOR 11 /* 次设备号*/
- #define TIMER1_INTERVAL (unsigned long)(0.01 * HZ)
- #define TIMER2_INTERVAL (unsigned long)(0.5 * HZ)
- #define PHYSICS_REG_ADDR 0x01C20800
- typedef struct
- {
- unsigned int CFG[4];
- unsigned int DAT ;
- unsigned int DRV0;
- unsigned int DRV1;
- unsigned int PUL0;
- unsigned int PUL1;
- }PIO_Struct;
- typedef struct
- {
- PIO_Struct Pn[7];
- }PIO_Map;
- typedef enum
- {
- PA = 0,
- PB = 1,
- PC = 2,
- PD = 3,
- PE = 4,
- PF = 5,
- PG = 6,
- }PORT;
- typedef enum
- {
- IN = 0x00,
- OUT = 0x01,
- AUX = 0x02,
- INT = 0x06,
- DISABLE = 0x07,
- }PIN_MODE;
- struct cdev *s_cdev;
- PIO_Map *PIO = NULL;
- struct timer_list Timer1,Timer2;
- static int StateLeft = 0,StateRight = 0;
- static int LeftCoderSpeed = 0,RightCoderSpeed = 0;
- static int LeftCoderCnt = 0,RightCoderCnt = 0;
- static int LeftCoderPin,RightCoderPin;
- static int LeftCoderPin_Pre,RightCoderPin_Pre;
- static void TIM1_Handler(unsigned long data);
- static void TIM2_Handler(unsigned long data);
- /*******************************************************************************
- ********************************************************************************
- GPIO操作
- ********************************************************************************
- ********************************************************************************/
- void GPIO_ConfigPin(PORT port,unsigned int pin,PIN_MODE mode)
- {
- PIO->Pn[port].CFG[pin / 8] &= ~((unsigned int)0x07 << pin % 8 * 4);
- PIO->Pn[port].CFG[pin / 8] |= ((unsigned int)mode << pin % 8 * 4);
- }
- void GPIO_SetPin(PORT port,unsigned int pin,unsigned int level)
- {
- if(level)
- PIO->Pn[port].DAT |= (1 << pin);
- else
- PIO->Pn[port].DAT &= ~(1 << pin);
- }
- unsigned int GPIO_GetPin(PORT port,unsigned int pin)
- {
- if(PIO->Pn[port].DAT & (1 << pin))
- return 1;
- else
- return 0;
- }
- /*******************************************************************************
- ********************************************************************************
- 驱动接口
- ********************************************************************************
- ********************************************************************************/
- static int device_open(struct inode *inode, struct file *filp)
- {
- // printk("coder_open\r\n");
- //创建内核定时器
- setup_timer(&Timer1, TIM1_Handler, 0);
- Timer1.expires = jiffies + TIMER1_INTERVAL;
-
- setup_timer(&Timer2, TIM2_Handler, 0);
- Timer2.expires = jiffies + TIMER2_INTERVAL;
-
- add_timer(&Timer1);
- add_timer(&Timer2);
-
- //映射物理内存到虚拟内存
- PIO = (PIO_Map *)ioremap(PHYSICS_REG_ADDR, 0x200);
- printk("VirtualAddr:%d\r\n",(unsigned int)PIO);
- GPIO_ConfigPin(PA,14,IN);
- GPIO_ConfigPin(PA,16,IN);
- LeftCoderPin = LeftCoderPin_Pre = GPIO_GetPin(PA,14);
- RightCoderPin = RightCoderPin_Pre = GPIO_GetPin(PA,16);
-
- return 0;
- }
- static int device_release(struct inode *inode, struct file *filp)
- {
- // printk("coder_release\r\n");
- del_timer(&Timer1);
- del_timer(&Timer2);
- if(PIO != NULL)
- iounmap((void *)PIO); //撤销映射关系
- return 0;
- }
- static int device_write(struct file *file, const char *buffer,size_t count, loff_t *ppos)
- {
- // printk("coder_write:%d\r\n",(int)count);
- return count;
- }
- static int device_read(struct file *file, char *buffer,size_t count, loff_t *ppos)
- {
- char str[2];
- // printk("coder_read\r\n");
- str[0] = (char)LeftCoderSpeed;
- str[1] = (char)RightCoderSpeed;
- copy_to_user(buffer, str, 2);
- return 2;
- }
- int device_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
- {
- return 0;
- }
- static struct file_operations fops =
- {
- .owner = THIS_MODULE,
- .compat_ioctl = device_ioctl,
- .open = device_open,
- .read = device_read,
- .write = device_write,
- .release = device_release,
- };
- static void TIM1_Handler(unsigned long data)
- {
- LeftCoderPin = GPIO_GetPin(PA,14);
- RightCoderPin = GPIO_GetPin(PA,16);
-
- if(StateLeft == 0)
- {
- if(LeftCoderPin != LeftCoderPin_Pre)
- {
- StateLeft = 1;
- }
- }
- else if(StateLeft == 1)
- {
- LeftCoderCnt++;
- StateLeft = 0;
- }
-
- if(StateRight == 0)
- {
- if(RightCoderPin != RightCoderPin_Pre)
- {
- StateRight = 1;
- }
- }
- else if(StateRight == 1)
- {
- RightCoderCnt++;
- StateRight = 0;
- }
-
- LeftCoderPin_Pre = LeftCoderPin;
- RightCoderPin_Pre = RightCoderPin;
-
- mod_timer(&Timer1, jiffies + TIMER1_INTERVAL);
- }
- static void TIM2_Handler(unsigned long data)
- {
- LeftCoderSpeed = LeftCoderCnt;
- LeftCoderCnt = 0;
- RightCoderSpeed = RightCoderCnt;
- RightCoderCnt = 0;
- mod_timer(&Timer2, jiffies + TIMER2_INTERVAL);
- }
- static int __init dev_init(void)
- {
- int result,err;
- dev_t devno;
-
- printk("device_init:%d\r\n",HZ);
- devno = MKDEV(DEVICE_MAJOR, DEVICE_MINOR);
- result = register_chrdev_region(devno, 1, DEVICE_NAME);
- s_cdev = kmalloc(sizeof(struct cdev), GFP_KERNEL);
- if (!s_cdev)
- {
- result = -ENOMEM;
- goto fail_malloc;
- }
- cdev_init(s_cdev, &fops);
- s_cdev->owner = THIS_MODULE;
- err = cdev_add(s_cdev, devno, 1);
-
- struct class *dev_class = class_create(THIS_MODULE,DEVICE_NAME);
- device_create(dev_class,NULL,devno,NULL,"zzz");
-
- return 0;
-
- fail_malloc:
- unregister_chrdev_region(devno, 1);
- return result;
- }
- static void __exit