微波EDA网,见证研发工程师的成长!
首页 > 研发问答 > 嵌入式设计讨论 > 嵌入式系统设计讨论 > 基于orangepi zero的智能小车 -- 结项

基于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的智能小车 -- 自动避障:

下面是小车的主程序:

  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <fcntl.h>
  4. #include <linux/i2c-dev.h>
  5. #include <errno.h>
  6. #include "gpio.h"  
  7. #include "i2c.h"
  8. #include "math.h"
  9. #include "pthread.h"
  10. #include <termios.h>
  11. #include <sys/timeb.h>
  12. #include <signal.h>        //signal()
  13. #include <string.h>        //memset()
  14. #include <sys/time.h>    //struct itimerval, setitimer()
  15. #include <semaphore.h>
  16. #include "./DMP/inv_mpu.h"
  17. #include "./DMP/inv_mpu_dmp_motion_driver.h"

  18. #define RIGHT_WHEEL                0
  19. #define LEFT_WHEEL                1

  20. typedef enum
  21. {
  22.         false = 0,
  23.         true = 1,
  24. }bool;

  25. static signed char gyro_orientation[9] = {-1, 0, 0,
  26.                                            0,-1, 0,
  27.                                            0, 0, 1};
  28. //fsr即Full Scale Range量程
  29. #define                                 q30  1073741824.0f
  30. float                                         q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
  31. char                                        num[50];
  32. float                                         Pitch,Roll,Yaw;
  33. unsigned long                         sensor_timestamp;
  34. short                                         gyro[3], accel[3], sensors;
  35. unsigned char                         more;
  36. long                                         quat[4];

  37. char                                        buf[256];

  38. unsigned char CoderSpeed[2];
  39. int coder_fd;
  40. int PWMLeft = 0,PWMRight = 0;

  41. bool IsAutoDrive = false;
  42. bool IsRailDrive = false;

  43. sem_t Sem_IIC;

  44. void p_interaction(void);
  45. void p_mpu6050reader(int signo);


  46. void DMP_Init(void)
  47. {
  48.         int i,result;
  49.         sem_wait(&Sem_IIC);        //申请信号量
  50.        
  51.         IIC_SetAddr(0x68);
  52.        
  53.         result = mpu_init();
  54.         if(!result)
  55.         {
  56.                 printf("mpu initialization complete......\n\r\n");
  57.                 //mpu_set_sensor                        设置传感器
  58.                 if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL))
  59.                         printf("mpu_set_sensor complete ......\n\r\n");
  60.                 else
  61.                         printf("mpu_set_sensor come across error ......\n\r\n");
  62.                 //mpu_configure_fifo                                                                        配置FIFO
  63.                 if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL))
  64.                         printf("mpu_configure_fifo complete ......\n\r\n");
  65.                 else
  66.                         printf("mpu_configure_fifo come across error ......\n\r\n");
  67.                 //mpu_set_sample_rate-----------------设置数据采样速率,默认是DEFAULT_MPU_HZ频率为100hz,在4hz到1000hz之间
  68.                 if(!mpu_set_sample_rate(iMPU_HZ))
  69.                         printf("mpu_set_sample_rate complete ......\n\r\n");
  70.                 else
  71.                         printf("mpu_set_sample_rate error ......\n\r\n");
  72.                 //dmp_load_motion_driver_firmvare-----加载运动引擎固件
  73.                 if(!dmp_load_motion_driver_firmware())
  74.                         printf("dmp_load_motion_driver_firmware complete ......\n\r\n");
  75.                 else
  76.                         printf("dmp_load_motion_driver_firmware come across error ......\n\r\n");
  77.                 //dmp_set_orientation-----------------设定方向
  78.                 if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))
  79.                         printf("dmp_set_orientation complete ......\n\r\n");
  80.                 else
  81.                         printf("dmp_set_orientation come across error ......\n\r\n");
  82.                 //dmp_enable_feature------------------使能功能特性
  83.                 if(!dmp_enable_feature(        DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT |
  84.                                                                 DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO | DMP_FEATURE_GYRO_CAL))
  85.                         printf("dmp_enable_feature complete ......\n\r\n");
  86.                 else
  87.                         printf("dmp_enable_feature come across error ......\n\r\n");
  88.                 //dmp_set_fifo_rate-------------------设置FIFO速率,默认是DEFAULT_MPU_HZ频率为100hz,采样速率不得大于 DMP_SAMPLE_RATE(200hz)
  89.                 if(!dmp_set_fifo_rate(iFIFO_HZ))
  90.                         printf("dmp_set_fifo_rate complete ......\n\r\n");
  91.                 else
  92.                         printf("dmp_set_fifo_rate come across error ......\n\r\n");
  93.                 run_self_test();
  94.                 if(!mpu_set_dmp_state(1))
  95.                         printf("mpu_set_dmp_state complete ......\n\r\n");
  96.                 else
  97.                         printf("mpu_set_dmp_state come across error ......\n\r\n");
  98.         }
  99.         else
  100.         {
  101.                 printf("mpu initialization error......\n\r\n");
  102.                 exit(0);
  103.         }
  104.        
  105.         sem_post(&Sem_IIC);        //释放信号量
  106. }

  107. void DataProcess(void)
  108. {       
  109.         sem_wait(&Sem_IIC);        //申请信号量
  110.        
  111.         IIC_SetAddr(0x68);
  112.         /* Gyro and accel data are written to the FIFO by the DMP in chip
  113.         * frame and hardware units. This behavior is convenient because it
  114.         * keeps the gyro and accel outputs of dmp_read_fifo and
  115.         * mpu_read_fifo consistent.
  116.         */
  117.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);         
  118.         /* Unlike gyro and accel, quaternions are written to the FIFO in
  119.         * the body frame, q30. The orientation is set by the scalar passed
  120.         * to dmp_set_orientation during initialization.
  121.         */
  122.         if(sensors & INV_WXYZ_QUAT)
  123.         {
  124.                 q0 = quat[0] / q30;
  125.                 q1 = quat[1] / q30;
  126.                 q2 = quat[2] / q30;
  127.                 q3 = quat[3] / q30;
  128.                 Pitch          = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3;        // pitch
  129.                 Roll        = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;        // roll
  130.                 Yaw                = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        // yaw没有实际用处,没有磁阻传感器会产生零飘
  131.         }
  132.        
  133.         sem_post(&Sem_IIC);        //释放信号量
  134. }

  135. void SetUpPCA9685()
  136. {
  137.         sem_wait(&Sem_IIC);        //申请信号量

  138.         IIC_SetAddr(0x40);
  139.         IIC_Write(0x00,0x10);
  140.         IIC_Write(0xfd,3);
  141.         IIC_Write(0x00,0x00);

  142.         int i;
  143.         for(i=6;i<=69;i++)
  144.         {
  145.                 IIC_Write((unsigned char)i,0x00);
  146.         }
  147.        
  148.         // for(i=0;i<=255;i++)
  149.         // {
  150.                 // printf("%.2X ",IIC_Read(i));
  151.                 // if((i+1 & 0x0F) == 0)
  152.                         // printf("\r\n");
  153.         // }

  154.         sem_post(&Sem_IIC);        //释放信号量
  155. }

  156. void PCA9685_SetPWM(int channel,unsigned short pwm)
  157. {
  158.         unsigned char h,l;
  159.         h = (unsigned char)(pwm >> 8);
  160.         l = (unsigned char)(pwm & 0xFF);
  161.         // printf("%d,%d\r\n",h,l);
  162.         IIC_Write((unsigned char)(channel * 4 + 0x08),l);
  163.         // printf("%.2X ",IIC_Read((unsigned char)(channel * 4 + 0x08)));
  164.         IIC_Write((unsigned char)(channel * 4 + 0x08 + 1),h);
  165.         // printf("%.2X\r\n",IIC_Read((unsigned char)(channel * 4 + 0x08 + 1)));
  166. }

  167. void SetWheel(int wheel,int pwm)
  168. {
  169.         unsigned short temp = (unsigned short)((pwm > 0)?pwm:-pwm);
  170.         sem_wait(&Sem_IIC);        //申请信号量

  171.         IIC_SetAddr(0x40);
  172.         // printf("%d,%d\r\n",wheel,pwm);
  173.         if(wheel == LEFT_WHEEL)
  174.         {
  175.                 if(pwm > 0)
  176.                 {
  177.                         PCA9685_SetPWM(2,0);
  178.                         PCA9685_SetPWM(3,temp);
  179.                 }
  180.                 else
  181.                 {
  182.                         PCA9685_SetPWM(2,temp);
  183.                         PCA9685_SetPWM(3,0);
  184.                 }
  185.         }
  186.         else
  187.         {
  188.                 if(pwm > 0)
  189.                 {
  190.                         PCA9685_SetPWM(0,temp);
  191.                         PCA9685_SetPWM(1,0);
  192.                 }
  193.                 else
  194.                 {
  195.                         PCA9685_SetPWM(0,0);
  196.                         PCA9685_SetPWM(1,temp);
  197.                 }
  198.         }
  199.        
  200.         sem_post(&Sem_IIC);        //释放信号量
  201. }

  202. void ReadCoder()
  203. {
  204.         read(coder_fd,CoderSpeed,2);
  205.         printf("%d,%d\r\n",CoderSpeed[LEFT_WHEEL],CoderSpeed[RIGHT_WHEEL]);
  206. }

  207. void TurnRight_Angle(int ang)
  208. {
  209.         int pre,dst,temp;
  210.         pre = (int)Yaw;
  211.         dst = pre - ang;
  212.         printf("pre:%d,dst:%d\r\n",pre,dst);
  213.        
  214.         SetWheel(LEFT_WHEEL,3000);
  215.         SetWheel(RIGHT_WHEEL,-3000);
  216.        
  217.         if(dst < -180)
  218.         {
  219.                 printf("dst < -180\r\n");
  220.                 temp =         360 - (-dst);
  221.                 printf("temp:%d\r\n",temp);
  222.                 while((int)Yaw < temp);
  223.                 printf("Yaw:%d\r\n",(int)Yaw);
  224.                 while((int)Yaw > temp);
  225.                 printf("Yaw:%d\r\n",(int)Yaw);
  226.         }
  227.         else
  228.         {
  229.                 printf("dst >= -180\r\n");
  230.                 while((int)Yaw > dst);
  231.                 printf("%d\r\n",(int)Yaw);
  232.         }
  233.         SetWheel(LEFT_WHEEL,-2000);
  234.         SetWheel(RIGHT_WHEEL,2000);
  235.         usleep(10000);
  236.         printf("ok\r\n\r\n");
  237.         SetWheel(LEFT_WHEEL,0);
  238.         SetWheel(RIGHT_WHEEL,0);
  239. }

  240. void TurnLeft_Angle(int ang)
  241. {
  242.         int pre,dst,temp;
  243.         pre = (int)Yaw;
  244.         dst = pre + ang;
  245.         printf("pre:%d,dst:%d\r\n",pre,dst);
  246.        
  247.         SetWheel(LEFT_WHEEL,-3000);
  248.         SetWheel(RIGHT_WHEEL,3000);
  249.        
  250.         if(dst > 180)
  251.         {
  252.                 printf("dst > 180\r\n");
  253.                 temp =         dst - 360;
  254.                 printf("temp:%d\r\n",temp);
  255.                 while((int)Yaw > temp);
  256.                 printf("Yaw:%d\r\n",(int)Yaw);
  257.                 while((int)Yaw < temp);
  258.                 printf("Yaw:%d\r\n",(int)Yaw);
  259.         }
  260.         else
  261.         {
  262.                 printf("dst <= 180\r\n");
  263.                 while((int)Yaw < dst);
  264.                 printf("%d\r\n",(int)Yaw);
  265.         }
  266.         SetWheel(LEFT_WHEEL,2000);
  267.         SetWheel(RIGHT_WHEEL,-2000);
  268.         usleep(10000);
  269.         printf("ok\r\n\r\n");
  270.         SetWheel(LEFT_WHEEL,0);
  271.         SetWheel(RIGHT_WHEEL,0);
  272. }

  273. void TurnRight(int tim)
  274. {
  275.         SetWheel(LEFT_WHEEL,3000);
  276.         SetWheel(RIGHT_WHEEL,-3000);
  277.         usleep(1000 * tim);
  278.         SetWheel(LEFT_WHEEL,0);
  279.         SetWheel(RIGHT_WHEEL,0);
  280. }

  281. void TurnLeft(int tim)
  282. {
  283.         SetWheel(LEFT_WHEEL,-3000);
  284.         SetWheel(RIGHT_WHEEL,3000);
  285.         usleep(1000 * tim);
  286.         SetWheel(LEFT_WHEEL,0);
  287.         SetWheel(RIGHT_WHEEL,0);
  288. }

  289. void Goback(int tim)
  290. {
  291.         SetWheel(LEFT_WHEEL,-3000);
  292.         SetWheel(RIGHT_WHEEL,-3000);
  293.         usleep(1000 * tim);
  294.         SetWheel(LEFT_WHEEL,0);
  295.         SetWheel(RIGHT_WHEEL,0);
  296. }

  297. static struct termios oldt;

  298. //restore terminal settings
  299. void restore_terminal_settings(void)
  300. {
  301.     //Apply saved settings
  302.     tcsetattr(0, TCSANOW, &oldt);
  303. }

  304. //make terminal read 1 char at a time
  305. void disable_terminal_return(void)
  306. {
  307.     struct termios newt;
  308.    
  309.     //save terminal settings
  310.     tcgetattr(0, &oldt);
  311.     //init new settings
  312.     newt = oldt;  
  313.     //change settings
  314.     newt.c_lflag &= ~(ICANON | ECHO);
  315.     //apply settings
  316.     tcsetattr(0, TCSANOW, &newt);
  317.    
  318.     //make sure settings will be restored when program ends
  319.     atexit(restore_terminal_settings);
  320. }

  321. int main(void)
  322. {       
  323.         disable_terminal_return();
  324.         GPIO_Init();
  325.         if((coder_fd = open("/dev/zzz",O_RDWR)) < 0)
  326.                 return;
  327.         // read(coder_fd,CoderSpeed,8);
  328.         // printf("%d,%d\r\n",CoderSpeed[LEFT_WHEEL],CoderSpeed[RIGHT_WHEEL]);
  329.        
  330.         GPIO_ConfigPin(PA,10,IN);
  331.         GPIO_ConfigPin(PA,13,IN);
  332.        
  333.         GPIO_ConfigPin(PA,18,IN);
  334.         GPIO_ConfigPin(PA,19,IN);
  335.        
  336.         sem_init(&Sem_IIC, 0, 1);
  337.        
  338.         //MPU6050的初始化工作必须在上点之后的几百毫秒之后,不然读出来的数据都是0,保险起见延时1s后初始化
  339.         //并且在每条初始化语句后面都要加上适当延时,不然还是会有数据不正常情况的发生.
  340.         usleep(1000000);                                                                       

  341.         int result;
  342.         result = IIC_Open();
  343.         SetUpPCA9685();
  344.         DMP_Init();
  345.        
  346.         pthread_t runner,MPU6050_Reader;
  347.     pthread_create(&runner,NULL,(void*)&p_interaction,NULL);
  348.     pthread_create(&MPU6050_Reader,NULL,(void*)&p_mpu6050reader,NULL);
  349.        
  350.         int a,b,c,d;
  351.         while(1)
  352.         {
  353.                 if(IsAutoDrive)
  354.                 {
  355.                         printf("Auto Drive\r\n");
  356.                         while(1)
  357.                         {
  358.                                 SetWheel(LEFT_WHEEL,2000);
  359.                                 SetWheel(RIGHT_WHEEL,2000);
  360.                                
  361.                                 a = GPIO_GetPin(PA,10);
  362.                                 b = GPIO_GetPin(PA,13);
  363.                                 if(a && !b)
  364.                                 {
  365.                                         TurnLeft(300);
  366.                                 }
  367.                                 else if(!a && b)
  368.                                 {
  369.                                         TurnRight(300);
  370.                                 }
  371.                                 else if(a && b)
  372.                                 {
  373.                                         TurnRight(1000);
  374.                                 }
  375.                                
  376.                                 if(!IsAutoDrive)
  377.                                 {
  378.                                         SetWheel(LEFT_WHEEL,0);
  379.                                         SetWheel(RIGHT_WHEEL,0);
  380.                                         break;
  381.                                 }
  382.                                 usleep(10000);
  383.                         }
  384.                 }
  385.                 else if(IsRailDrive)
  386.                 {
  387.                         printf("Rail Drive\r\n");
  388.                         while(1)
  389.                         {
  390.                                 a = GPIO_GetPin(PA,18);
  391.                                 b = GPIO_GetPin(PA,19);
  392.                                 c = GPIO_GetPin(PA,10);
  393.                                 d = GPIO_GetPin(PA,13);
  394.                                 if(c || d)
  395.                                 {
  396.                                         SetWheel(LEFT_WHEEL,0);
  397.                                         SetWheel(RIGHT_WHEEL,0);
  398.                                 }
  399.                                 else
  400.                                 {
  401.                                         if(!a && b)
  402.                                         {
  403.                                                 SetWheel(LEFT_WHEEL,0);
  404.                                                 SetWheel(RIGHT_WHEEL,4000);
  405.                                                 while(1)
  406.                                                 {
  407.                                                         a = GPIO_GetPin(PA,18);
  408.                                                         b = GPIO_GetPin(PA,19);
  409.                                                         if(!(a ^ b))
  410.                                                         {
  411.                                                                 break;
  412.                                                         }
  413.                                                 }
  414.                                         }
  415.                                         else if(a && !b)
  416.                                         {
  417.                                                 SetWheel(LEFT_WHEEL,4000);
  418.                                                 SetWheel(RIGHT_WHEEL,0);
  419.                                                 while(1)
  420.                                                 {
  421.                                                         a = GPIO_GetPin(PA,18);
  422.                                                         b = GPIO_GetPin(PA,19);
  423.                                                         if(!(a ^ b))
  424.                                                         {
  425.                                                                 break;
  426.                                                         }
  427.                                                 }
  428.                                         }
  429.                                         else
  430.                                         {
  431.                                                 SetWheel(LEFT_WHEEL,2000);
  432.                                                 SetWheel(RIGHT_WHEEL,2000);
  433.                                         }
  434.                                 }
  435.                                
  436.                                 if(!IsRailDrive)
  437.                                 {
  438.                                         SetWheel(LEFT_WHEEL,0);
  439.                                         SetWheel(RIGHT_WHEEL,0);
  440.                                         break;
  441.                                 }
  442.                                 usleep(10000);
  443.                         }
  444.                 }
  445.         }
  446. }

  447. void p_interaction(void)
  448. {
  449.         char cmd_char;
  450.         while(1)
  451.         {
  452.                 cmd_char = (char)getchar();
  453.                 // printf("%c",cmd_char);
  454.                 // fflush(stdout);
  455.                
  456.                 if(IsAutoDrive || IsRailDrive)
  457.                 {
  458.                         if(cmd_char == 'e')
  459.                         {
  460.                                 IsAutoDrive = false;
  461.                                 PWMLeft = 0;
  462.                                 PWMRight = 0;
  463.                         }
  464.                         if(cmd_char == 'r')
  465.                         {
  466.                                 IsRailDrive = false;
  467.                                 PWMLeft = 0;
  468.                                 PWMRight = 0;
  469.                         }
  470.                 }
  471.                 else
  472.                 {
  473.                         if(cmd_char == 'w')
  474.                         {
  475.                                 PWMLeft = 3000;
  476.                                 PWMRight = 3000;
  477.                         }
  478.                         else if(cmd_char == 'a')
  479.                         {
  480.                                 PWMLeft = -3000;
  481.                                 PWMRight = 3000;
  482.                         }
  483.                         else if(cmd_char == 's')
  484.                         {
  485.                                 PWMLeft = -3000;
  486.                                 PWMRight = -3000;
  487.                         }
  488.                         else if(cmd_char == 'd')
  489.                         {
  490.                                 PWMLeft = 3000;
  491.                                 PWMRight = -3000;
  492.                         }
  493.                         else if(cmd_char == 'q')
  494.                         {
  495.                                 PWMLeft = 4000;
  496.                                 PWMRight = 4000;
  497.                         }
  498.                         else if(cmd_char == ' ')
  499.                         {
  500.                                 PWMLeft = 0;
  501.                                 PWMRight = 0;
  502.                         }
  503.                         else if(cmd_char == 'e')
  504.                         {
  505.                                 IsAutoDrive = true;
  506.                                 PWMLeft = 0;
  507.                                 PWMRight = 0;
  508.                         }
  509.                         else if(cmd_char == 'r')
  510.                         {
  511.                                 IsRailDrive = true;
  512.                                 PWMLeft = 0;
  513.                                 PWMRight = 0;
  514.                         }
  515.                         SetWheel(LEFT_WHEEL,PWMLeft);
  516.                         SetWheel(RIGHT_WHEEL,PWMRight);
  517.                 }
  518.         }
  519. }

  520. void p_mpu6050reader(int signo)
  521. {
  522.         while(1)
  523.         {
  524.                 DataProcess();
  525.                 // printf("%.4f,%.4f,%.4f\r\n",Pitch,Roll,Yaw);
  526.                 usleep(10000);
  527.         }
  528. }

复制代码


下面是电机编码盘的驱动程序:

  1. #include <linux/errno.h>
  2. #include <linux/kernel.h>
  3. #include <linux/module.h>
  4. #include <linux/slab.h>
  5. #include <linux/input.h>
  6. #include <linux/init.h>
  7. #include <linux/serio.h>
  8. #include <linux/delay.h>
  9. #include <linux/clk.h>
  10. #include <asm/io.h>
  11. #include <asm/irq.h>
  12. #include <asm/uaccess.h>
  13. #include <linux/cdev.h>
  14. #include <linux/miscdevice.h>
  15. #include <linux/timer.h>

  16. #define DEVICE_NAME                        "CODER"
  17. #define DEVICE_MAJOR                246                /* 主设备号*/
  18. #define DEVICE_MINOR                11                /* 次设备号*/

  19. #define TIMER1_INTERVAL                (unsigned long)(0.01 * HZ)
  20. #define TIMER2_INTERVAL                (unsigned long)(0.5 * HZ)
  21. #define PHYSICS_REG_ADDR        0x01C20800


  22. typedef struct
  23. {
  24.         unsigned int CFG[4];
  25.         unsigned int DAT ;
  26.         unsigned int DRV0;
  27.         unsigned int DRV1;
  28.         unsigned int PUL0;
  29.         unsigned int PUL1;
  30. }PIO_Struct;

  31. typedef struct
  32. {
  33.         PIO_Struct Pn[7];
  34. }PIO_Map;

  35. typedef enum
  36. {
  37.         PA = 0,
  38.         PB = 1,
  39.         PC = 2,
  40.         PD = 3,
  41.         PE = 4,
  42.         PF = 5,
  43.         PG = 6,
  44. }PORT;

  45. typedef enum
  46. {
  47.         IN                        = 0x00,
  48.         OUT                        = 0x01,
  49.         AUX                        = 0x02,
  50.         INT                        = 0x06,
  51.         DISABLE                = 0x07,
  52. }PIN_MODE;

  53. struct cdev *s_cdev;
  54. PIO_Map *PIO = NULL;
  55. struct timer_list Timer1,Timer2;
  56. static int StateLeft = 0,StateRight = 0;
  57. static int LeftCoderSpeed = 0,RightCoderSpeed = 0;
  58. static int LeftCoderCnt = 0,RightCoderCnt = 0;
  59. static int LeftCoderPin,RightCoderPin;
  60. static int LeftCoderPin_Pre,RightCoderPin_Pre;

  61. static void TIM1_Handler(unsigned long data);
  62. static void TIM2_Handler(unsigned long data);


  63. /*******************************************************************************
  64. ********************************************************************************
  65.                                                                         GPIO操作
  66. ********************************************************************************
  67. ********************************************************************************/
  68. void GPIO_ConfigPin(PORT port,unsigned int pin,PIN_MODE mode)
  69. {
  70.         PIO->Pn[port].CFG[pin / 8] &= ~((unsigned int)0x07 << pin % 8 * 4);
  71.         PIO->Pn[port].CFG[pin / 8] |=  ((unsigned int)mode << pin % 8 * 4);
  72. }

  73. void GPIO_SetPin(PORT port,unsigned int pin,unsigned int level)
  74. {
  75.         if(level)
  76.                 PIO->Pn[port].DAT |= (1 << pin);
  77.         else
  78.                 PIO->Pn[port].DAT &= ~(1 << pin);
  79. }

  80. unsigned int GPIO_GetPin(PORT port,unsigned int pin)
  81. {
  82.         if(PIO->Pn[port].DAT & (1 << pin))
  83.                 return 1;
  84.         else
  85.                 return 0;
  86. }



  87. /*******************************************************************************
  88. ********************************************************************************
  89.                                                                         驱动接口
  90. ********************************************************************************
  91. ********************************************************************************/
  92. static int device_open(struct inode *inode, struct file *filp)
  93. {
  94.         // printk("coder_open\r\n");
  95.         //创建内核定时器
  96.         setup_timer(&Timer1, TIM1_Handler, 0);
  97.         Timer1.expires = jiffies + TIMER1_INTERVAL;
  98.        
  99.         setup_timer(&Timer2, TIM2_Handler, 0);
  100.         Timer2.expires = jiffies + TIMER2_INTERVAL;
  101.        
  102.         add_timer(&Timer1);
  103.         add_timer(&Timer2);
  104.        
  105.         //映射物理内存到虚拟内存
  106.     PIO = (PIO_Map *)ioremap(PHYSICS_REG_ADDR, 0x200);
  107.         printk("VirtualAddr:%d\r\n",(unsigned int)PIO);
  108.         GPIO_ConfigPin(PA,14,IN);
  109.         GPIO_ConfigPin(PA,16,IN);
  110.         LeftCoderPin = LeftCoderPin_Pre = GPIO_GetPin(PA,14);
  111.         RightCoderPin = RightCoderPin_Pre = GPIO_GetPin(PA,16);
  112.        
  113.         return 0;
  114. }

  115. static int device_release(struct inode *inode, struct file *filp)
  116. {
  117.         // printk("coder_release\r\n");
  118.         del_timer(&Timer1);
  119.         del_timer(&Timer2);
  120.         if(PIO != NULL)
  121.                 iounmap((void *)PIO); //撤销映射关系
  122.         return 0;
  123. }

  124. static int device_write(struct file *file, const char *buffer,size_t count, loff_t *ppos)
  125. {
  126.         // printk("coder_write:%d\r\n",(int)count);
  127.         return count;
  128. }

  129. static int device_read(struct file *file, char *buffer,size_t count, loff_t *ppos)
  130. {
  131.         char str[2];
  132.         // printk("coder_read\r\n");
  133.         str[0] = (char)LeftCoderSpeed;
  134.         str[1] = (char)RightCoderSpeed;
  135.         copy_to_user(buffer, str, 2);
  136.     return 2;
  137. }

  138. int device_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
  139. {
  140.     return 0;
  141. }
  142. static struct file_operations fops =
  143. {
  144.         .owner                        =        THIS_MODULE,
  145.         .compat_ioctl        =        device_ioctl,
  146.         .open                        =        device_open,
  147.         .read                        =        device_read,       
  148.         .write                        =        device_write,       
  149.         .release                =        device_release,
  150. };

  151. static void TIM1_Handler(unsigned long data)
  152. {
  153.         LeftCoderPin = GPIO_GetPin(PA,14);
  154.         RightCoderPin = GPIO_GetPin(PA,16);
  155.        
  156.         if(StateLeft == 0)
  157.         {
  158.                 if(LeftCoderPin != LeftCoderPin_Pre)
  159.                 {
  160.                         StateLeft = 1;
  161.                 }
  162.         }
  163.         else if(StateLeft == 1)
  164.         {
  165.                 LeftCoderCnt++;
  166.                 StateLeft = 0;
  167.         }
  168.        
  169.         if(StateRight == 0)
  170.         {
  171.                 if(RightCoderPin != RightCoderPin_Pre)
  172.                 {
  173.                         StateRight = 1;
  174.                 }
  175.         }
  176.         else if(StateRight == 1)
  177.         {
  178.                 RightCoderCnt++;
  179.                 StateRight = 0;
  180.         }
  181.        
  182.         LeftCoderPin_Pre        = LeftCoderPin;
  183.         RightCoderPin_Pre        = RightCoderPin;
  184.        
  185.         mod_timer(&Timer1, jiffies + TIMER1_INTERVAL);
  186. }

  187. static void TIM2_Handler(unsigned long data)
  188. {
  189.         LeftCoderSpeed = LeftCoderCnt;
  190.         LeftCoderCnt = 0;
  191.         RightCoderSpeed = RightCoderCnt;
  192.         RightCoderCnt = 0;
  193.         mod_timer(&Timer2, jiffies + TIMER2_INTERVAL);
  194. }

  195. static int __init dev_init(void)
  196. {
  197.         int result,err;
  198.         dev_t devno;
  199.        
  200.         printk("device_init:%d\r\n",HZ);

  201.         devno = MKDEV(DEVICE_MAJOR, DEVICE_MINOR);
  202.         result = register_chrdev_region(devno, 1, DEVICE_NAME);
  203.         s_cdev = kmalloc(sizeof(struct cdev), GFP_KERNEL);
  204.         if (!s_cdev)
  205.         {
  206.                 result = -ENOMEM;
  207.                 goto fail_malloc;
  208.         }
  209.         cdev_init(s_cdev, &fops);
  210.         s_cdev->owner = THIS_MODULE;
  211.         err = cdev_add(s_cdev, devno, 1);
  212.        
  213.         struct class *dev_class = class_create(THIS_MODULE,DEVICE_NAME);
  214.         device_create(dev_class,NULL,devno,NULL,"zzz");
  215.        
  216.         return 0;
  217.        
  218. fail_malloc:
  219.         unregister_chrdev_region(devno, 1);
  220.         return result;
  221. }

  222. static void __exit

Copyright © 2017-2020 微波EDA网 版权所有

网站地图

Top