嵌入式技术论坛
直播中

汤权

11年用户 213经验值
擅长:模拟技术 RF/无线
私信 关注
[经验]

【orangepi zero试用体验】基于orangepi zero的智能小车 -- 结项

本帖最后由 384998430 于 2017-1-17 22:11 编辑

      小弟我也是第一次在电子发烧友上申请到板子使用，十分感激发烧友为我提供的orangepi zero开发板，希望能够做一次好的评测，但是由于小弟我水平有限，加上年前各种事情很忙很忙，可能我的评测并没有大家期望的结果，不过还是希望我的评测能够帮助到论坛上的学友们，希望大家不吝赐教。
      首先我是第一次用嵌入式Linux开发板进行硬件开发，之前做这些小车之类的“玩具”都是使用STM32、51单片机之类的处理器，虽然STM32也能跑各种嵌入式实时操作系统，但是和Linux的编程方式还是有很大的不同，在Linux系统下对于GPIO的操作都会复杂很多，但是Linux也提供了更多的功能，这也不是STM32之类的处理器能够比拟的。
      开发初期的话是对开发环境和开发过程的熟悉，包括开发板的系统安装，编辑器VIM的使用，系统软件的安装，NFS服务器的搭建，随后进行GPIO控制、IIC总线测试、串口读取测试、车模电机驱动、电机编码盘驱动编写，最后到进行车模进行行走的程序设计。
      前面的帖子总会如下：
https://bbs.elecfans.com/forum.ph ... &tid=1100804&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1101528&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1101718&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1102155&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1102353&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1102458&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1102824&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1104313&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1105396&extra=
https://bbs.elecfans.com/forum.ph ... &tid=1106766&extra=
      我为车子加了6轴姿态传感器、4个红外感应传感器、两个编码盘、一个电机启动模块、GPS模块（由于一直在室内进行测试，无法接受到GPS信号，所以GPS模块没有使用），车子整体如下图：

IMG_20170114_232958.jpg



IMG_20170114_233005.jpg


IMG_20170114_233023.jpg


IMG_20170114_233056_HHT.jpg

四个红外传感器两个用于检测前方有没有障碍物，用于自动避障，两个用于检测地面黑线，用于寻迹。整个系统使用一个4000mah的充电宝作为电源供电，可以长时间运行。车子有三种工作模式，第一种是人工遥控控制小车运行，第二种是小车沿着黑线的位置进行寻迹，最后一种是自动避障运行,下面是三种运行模式的视频链接，我是在租的小房间里面实验的，没法找到大场地。基于orangepi zero的智能小车 -- 手动遥控：
[media]http://v.youku.com/v_show/id_XMjQ3MzA1NTAwOA==.html?spm=a2hzp.8253869.0.0&from=y1.7-2[/media]

基于orangepi zero的智能小车 -- 寻迹：
[media]http://v.youku.com/v_show/id_XMjQ3MzA1ODM2MA==.html?spm=a2hzp.8253869.0.0&from=y1.7-2[/media]

基于orangepi zero的智能小车 -- 自动避障：
[media]http://v.youku.com/v_show/id_XMjQ3MzA1ODk4NA==.html?spm=a2hzp.8253869.0.0&from=y1.7-2[/media]

下面是小车的主程序：
  1. #include
  2. #include
  3. #include
  4. #include
  5. #include
  6. #include "gpio.h"  
  7. #include "i2c.h"
  8. #include "math.h"
  9. #include "pthread.h"
  10. #include
  11. #include timeb.h>
  12. #include         //signal()
  13. #include         //memset()
  14. #include     //struct itimerval, setitimer()
  15. #include
  16. #include "./DMP/inv_mpu.h"
  17. #include "./DMP/inv_mpu_dmp_motion_driver.h"

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

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

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

  41. char                                        buf[256];

  43. unsigned char CoderSpeed[2];
  44. int coder_fd;
  45. int PWMLeft = 0,PWMRight = 0;

  47. bool IsAutoDrive = false;
  48. bool IsRailDrive = false;

  50. sem_t Sem_IIC;

  52. void p_interaction(void);
  53. void p_mpu6050reader(int signo);


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

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

  147. void SetUpPCA9685()
  148. {
  149.         sem_wait(&Sem_IIC);        //申请信号量

  151.         IIC_SetAddr(0x40);
  152.         IIC_Write(0x00,0x10);
  153.         IIC_Write(0xfd,3);
  154.         IIC_Write(0x00,0x00);

  156.         int i;
  157.         for(i=6;i<=69;i++)
  158.         {
  159.                 IIC_Write((unsigned char)i,0x00);
  160.         }
  161.        
  162.         // for(i=0;i<=255;i++)
  163.         // {
  164.                 // printf("%.2X ",IIC_Read(i));
  165.                 // if((i+1 & 0x0F) == 0)
  166.                         // printf("rn");
  167.         // }

  169.         sem_post(&Sem_IIC);        //释放信号量
  170. }

  172. void PCA9685_SetPWM(int channel,unsigned short pwm)
  173. {
  174.         unsigned char h,l;
  175.         h = (unsigned char)(pwm >> 8);
  176.         l = (unsigned char)(pwm & 0xFF);
  177.         // printf("%d,%drn",h,l);
  178.         IIC_Write((unsigned char)(channel * 4 + 0x08),l);
  179.         // printf("%.2X ",IIC_Read((unsigned char)(channel * 4 + 0x08)));
  180.         IIC_Write((unsigned char)(channel * 4 + 0x08 + 1),h);
  181.         // printf("%.2Xrn",IIC_Read((unsigned char)(channel * 4 + 0x08 + 1)));
  182. }

  184. void SetWheel(int wheel,int pwm)
  185. {
  186.         unsigned short temp = (unsigned short)((pwm > 0)?pwm:-pwm);
  187.         sem_wait(&Sem_IIC);        //申请信号量

  189.         IIC_SetAddr(0x40);
  190.         // printf("%d,%drn",wheel,pwm);
  191.         if(wheel == LEFT_WHEEL)
  192.         {
  193.                 if(pwm > 0)
  194.                 {
  195.                         PCA9685_SetPWM(2,0);
  196.                         PCA9685_SetPWM(3,temp);
  197.                 }
  198.                 else
  199.                 {
  200.                         PCA9685_SetPWM(2,temp);
  201.                         PCA9685_SetPWM(3,0);
  202.                 }
  203.         }
  204.         else
  205.         {
  206.                 if(pwm > 0)
  207.                 {
  208.                         PCA9685_SetPWM(0,temp);
  209.                         PCA9685_SetPWM(1,0);
  210.                 }
  211.                 else
  212.                 {
  213.                         PCA9685_SetPWM(0,0);
  214.                         PCA9685_SetPWM(1,temp);
  215.                 }
  216.         }
  217.        
  218.         sem_post(&Sem_IIC);        //释放信号量
  219. }

  221. void ReadCoder()
  222. {
  223.         read(coder_fd,CoderSpeed,2);
  224.         printf("%d,%drn",CoderSpeed[LEFT_WHEEL],CoderSpeed[RIGHT_WHEEL]);
  225. }

  227. void TurnRight_Angle(int ang)
  228. {
  229.         int pre,dst,temp;
  230.         pre = (int)Yaw;
  231.         dst = pre - ang;
  232.         printf("pre:%d,dst:%drn",pre,dst);
  233.        
  234.         SetWheel(LEFT_WHEEL,3000);
  235.         SetWheel(RIGHT_WHEEL,-3000);
  236.        
  237.         if(dst < -180)
  238.         {
  239.                 printf("dst < -180rn");
  240.                 temp =         360 - (-dst);
  241.                 printf("temp:%drn",temp);
  242.                 while((int)Yaw < temp);
  243.                 printf("Yaw:%drn",(int)Yaw);
  244.                 while((int)Yaw > temp);
  245.                 printf("Yaw:%drn",(int)Yaw);
  246.         }
  247.         else
  248.         {
  249.                 printf("dst >= -180rn");
  250.                 while((int)Yaw > dst);
  251.                 printf("%drn",(int)Yaw);
  252.         }
  253.         SetWheel(LEFT_WHEEL,-2000);
  254.         SetWheel(RIGHT_WHEEL,2000);
  255.         usleep(10000);
  256.         printf("okrnrn");
  257.         SetWheel(LEFT_WHEEL,0);
  258.         SetWheel(RIGHT_WHEEL,0);
  259. }

  261. void TurnLeft_Angle(int ang)
  262. {
  263.         int pre,dst,temp;
  264.         pre = (int)Yaw;
  265.         dst = pre + ang;
  266.         printf("pre:%d,dst:%drn",pre,dst);
  267.        
  268.         SetWheel(LEFT_WHEEL,-3000);
  269.         SetWheel(RIGHT_WHEEL,3000);
  270.        
  271.         if(dst > 180)
  272.         {
  273.                 printf("dst > 180rn");
  274.                 temp =         dst - 360;
  275.                 printf("temp:%drn",temp);
  276.                 while((int)Yaw > temp);
  277.                 printf("Yaw:%drn",(int)Yaw);
  278.                 while((int)Yaw < temp);
  279.                 printf("Yaw:%drn",(int)Yaw);
  280.         }
  281.         else
  282.         {
  283.                 printf("dst <= 180rn");
  284.                 while((int)Yaw < dst);
  285.                 printf("%drn",(int)Yaw);
  286.         }
  287.         SetWheel(LEFT_WHEEL,2000);
  288.         SetWheel(RIGHT_WHEEL,-2000);
  289.         usleep(10000);
  290.         printf("okrnrn");
  291.         SetWheel(LEFT_WHEEL,0);
  292.         SetWheel(RIGHT_WHEEL,0);
  293. }

  295. void TurnRight(int tim)
  296. {
  297.         SetWheel(LEFT_WHEEL,3000);
  298.         SetWheel(RIGHT_WHEEL,-3000);
  299.         usleep(1000 * tim);
  300.         SetWheel(LEFT_WHEEL,0);
  301.         SetWheel(RIGHT_WHEEL,0);
  302. }

  304. void TurnLeft(int tim)
  305. {
  306.         SetWheel(LEFT_WHEEL,-3000);
  307.         SetWheel(RIGHT_WHEEL,3000);
  308.         usleep(1000 * tim);
  309.         SetWheel(LEFT_WHEEL,0);
  310.         SetWheel(RIGHT_WHEEL,0);
  311. }

  313. void Goback(int tim)
  314. {
  315.         SetWheel(LEFT_WHEEL,-3000);
  316.         SetWheel(RIGHT_WHEEL,-3000);
  317.         usleep(1000 * tim);
  318.         SetWheel(LEFT_WHEEL,0);
  319.         SetWheel(RIGHT_WHEEL,0);
  320. }

  322. static struct termios oldt;

  324. //restore terminal settings
  325. void restore_terminal_settings(void)
  326. {
  327.     //Apply saved settings
  328.     tcsetattr(0, TCSANOW, &oldt);
  329. }

  331. //make terminal read 1 char at a time
  332. void disable_terminal_return(void)
  333. {
  334.     struct termios newt;
  335.    
  336.     //save terminal settings
  337.     tcgetattr(0, &oldt);
  338.     //init new settings
  339.     newt = oldt;  
  340.     //change settings
  341.     newt.c_lflag &= ~(ICANON | ECHO);
  342.     //apply settings
  343.     tcsetattr(0, TCSANOW, &newt);
  344.    
  345.     //make sure settings will be restored when program ends
  346.     atexit(restore_terminal_settings);
  347. }

  349. int main(void)
  350. {       
  351.         disable_terminal_return();
  352.         GPIO_Init();
  353.         if((coder_fd = open("/dev/zzz",O_RDWR)) < 0)
  354.                 return;
  355.         // read(coder_fd,CoderSpeed,8);
  356.         // printf("%d,%drn",CoderSpeed[LEFT_WHEEL],CoderSpeed[RIGHT_WHEEL]);
  357.        
  358.         GPIO_ConfigPin(PA,10,IN);
  359.         GPIO_ConfigPin(PA,13,IN);
  360.        
  361.         GPIO_ConfigPin(PA,18,IN);
  362.         GPIO_ConfigPin(PA,19,IN);
  363.        
  364.         sem_init(&Sem_IIC, 0, 1);
  365.        
  366.         //MPU6050的初始化工作必须在上点之后的几百毫秒之后,不然读出来的数据都是0,保险起见延时1s后初始化
  367.         //并且在每条初始化语句后面都要加上适当延时,不然还是会有数据不正常情况的发生.
  368.         usleep(1000000);                                                                       

  370.         int result;
  371.         result = IIC_Open();
  372.         SetUpPCA9685();
  373.         DMP_Init();
  374.        
  375.         pthread_t runner,MPU6050_Reader;
  376.     pthread_create(&runner,NULL,(void*)&p_interaction,NULL);
  377.     pthread_create(&MPU6050_Reader,NULL,(void*)&p_mpu6050reader,NULL);
  378.        
  379.         int a,b,c,d;
  380.         while(1)
  381.         {
  382.                 if(IsAutoDrive)
  383.                 {
  384.                         printf("Auto Drivern");
  385.                         while(1)
  386.                         {
  387.                                 SetWheel(LEFT_WHEEL,2000);
  388.                                 SetWheel(RIGHT_WHEEL,2000);
  389.                                
  390.                                 a = GPIO_GetPin(PA,10);
  391.                                 b = GPIO_GetPin(PA,13);
  392.                                 if(a && !b)
  393.                                 {
  394.                                         TurnLeft(300);
  395.                                 }
  396.                                 else if(!a && b)
  397.                                 {
  398.                                         TurnRight(300);
  399.                                 }
  400.                                 else if(a && b)
  401.                                 {
  402.                                         TurnRight(1000);
  403.                                 }
  404.                                
  405.                                 if(!IsAutoDrive)
  406.                                 {
  407.                                         SetWheel(LEFT_WHEEL,0);
  408.                                         SetWheel(RIGHT_WHEEL,0);
  409.                                         break;
  410.                                 }
  411.                                 usleep(10000);
  412.                         }
  413.                 }
  414.                 else if(IsRailDrive)
  415.                 {
  416.                         printf("Rail Drivern");
  417.                         while(1)
  418.                         {
  419.                                 a = GPIO_GetPin(PA,18);
  420.                                 b = GPIO_GetPin(PA,19);
  421.                                 c = GPIO_GetPin(PA,10);
  422.                                 d = GPIO_GetPin(PA,13);
  423.                                 if(c || d)
  424.                                 {
  425.                                         SetWheel(LEFT_WHEEL,0);
  426.                                         SetWheel(RIGHT_WHEEL,0);
  427.                                 }
  428.                                 else
  429.                                 {
  430.                                         if(!a && b)
  431.                                         {
  432.                                                 SetWheel(LEFT_WHEEL,0);
  433.                                                 SetWheel(RIGHT_WHEEL,4000);
  434.                                                 while(1)
  435.                                                 {
  436.                                                         a = GPIO_GetPin(PA,18);
  437.                                                         b = GPIO_GetPin(PA,19);
  438.                                                         if(!(a ^ b))
  439.                                                         {
  440.                                                                 break;
  441.                                                         }
  442.                                                 }
  443.                                         }
  444.                                         else if(a && !b)
  445.                                         {
  446.                                                 SetWheel(LEFT_WHEEL,4000);
  447.                                                 SetWheel(RIGHT_WHEEL,0);
  448.                                                 while(1)
  449.                                                 {
  450.                                                         a = GPIO_GetPin(PA,18);
  451.                                                         b = GPIO_GetPin(PA,19);
  452.                                                         if(!(a ^ b))
  453.                                                         {
  454.                                                                 break;
  455.                                                         }
  456.                                                 }
  457.                                         }
  458.                                         else
  459.                                         {
  460.                                                 SetWheel(LEFT_WHEEL,2000);
  461.                                                 SetWheel(RIGHT_WHEEL,2000);
  462.                                         }
  463.                                 }
  464.                                
  465.                                 if(!IsRailDrive)
  466.                                 {
  467.                                         SetWheel(LEFT_WHEEL,0);
  468.                                         SetWheel(RIGHT_WHEEL,0);
  469.                                         break;
  470.                                 }
  471.                                 usleep(10000);
  472.                         }
  473.                 }
  474.         }
  475. }

  477. void p_interaction(void)
  478. {
  479.         char cmd_char;
  480.         while(1)
  481.         {
  482.                 cmd_char = (char)getchar();
  483.                 // printf("%c",cmd_char);
  484.                 // fflush(stdout);
  485.                
  486.                 if(IsAutoDrive || IsRailDrive)
  487.                 {
  488.                         if(cmd_char == 'e')
  489.                         {
  490.                                 IsAutoDrive = false;
  491.                                 PWMLeft = 0;
  492.                                 PWMRight = 0;
  493.                         }
  494.                         if(cmd_char == 'r')
  495.                         {
  496.                                 IsRailDrive = false;
  497.                                 PWMLeft = 0;
  498.                                 PWMRight = 0;
  499.                         }
  500.                 }
  501.                 else
  502.                 {
  503.                         if(cmd_char == 'w')
  504.                         {
  505.                                 PWMLeft = 3000;
  506.                                 PWMRight = 3000;
  507.                         }
  508.                         else if(cmd_char == 'a')
  509.                         {
  510.                                 PWMLeft = -3000;
  511.                                 PWMRight = 3000;
  512.                         }
  513.                         else if(cmd_char == 's')
  514.                         {
  515.                                 PWMLeft = -3000;
  516.                                 PWMRight = -3000;
  517.                         }
  518.                         else if(cmd_char == 'd')
  519.                         {
  520.                                 PWMLeft = 3000;
  521.                                 PWMRight = -3000;
  522.                         }
  523.                         else if(cmd_char == 'q')
  524.                         {
  525.                                 PWMLeft = 4000;
  526.                                 PWMRight = 4000;
  527.                         }
  528.                         else if(cmd_char == ' ')
  529.                         {
  530.                                 PWMLeft = 0;
  531.                                 PWMRight = 0;
  532.                         }
  533.                         else if(cmd_char == 'e')
  534.                         {
  535.                                 IsAutoDrive = true;
  536.                                 PWMLeft = 0;
  537.                                 PWMRight = 0;
  538.                         }
  539.                         else if(cmd_char == 'r')
  540.                         {
  541.                                 IsRailDrive = true;
  542.                                 PWMLeft = 0;
  543.                                 PWMRight = 0;
  544.                         }
  545.                         SetWheel(LEFT_WHEEL,PWMLeft);
  546.                         SetWheel(RIGHT_WHEEL,PWMRight);
  547.                 }
  548.         }
  549. }

  551. void p_mpu6050reader(int signo)
  552. {
  553.         while(1)
  554.         {
  555.                 DataProcess();
  556.                 // printf("%.4f,%.4f,%.4frn",Pitch,Roll,Yaw);
  557.                 usleep(10000);
  558.         }
  559. }



下面是电机编码盘的驱动程序：
  1. #include
  2. #include
  3. #include
  4. #include
  5. #include
  6. #include
  7. #include
  8. #include
  9. #include
  10. #include
  11. #include
  12. #include
  13. #include
  14. #include
  15. #include

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

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


  26. typedef struct
  27. {
  28.         unsigned int CFG[4];
  29.         unsigned int DAT ;
  30.         unsigned int DRV0;
  31.         unsigned int DRV1;
  32.         unsigned int PUL0;
  33.         unsigned int PUL1;
  34. }PIO_Struct;

  36. typedef struct
  37. {
  38.         PIO_Struct Pn[7];
  39. }PIO_Map;

  41. typedef enum
  42. {
  43.         PA = 0,
  44.         PB = 1,
  45.         PC = 2,
  46.         PD = 3,
  47.         PE = 4,
  48.         PF = 5,
  49.         PG = 6,
  50. }PORT;

  52. typedef enum
  53. {
  54.         IN                        = 0x00,
  55.         OUT                        = 0x01,
  56.         AUX                        = 0x02,
  57.         INT                        = 0x06,
  58.         DISABLE                = 0x07,
  59. }PIN_MODE;

  61. struct cdev *s_cdev;
  62. PIO_Map *PIO = NULL;
  63. struct timer_list Timer1,Timer2;
  64. static int StateLeft = 0,StateRight = 0;
  65. static int LeftCoderSpeed = 0,RightCoderSpeed = 0;
  66. static int LeftCoderCnt = 0,RightCoderCnt = 0;
  67. static int LeftCoderPin,RightCoderPin;
  68. static int LeftCoderPin_Pre,RightCoderPin_Pre;

  70. static void TIM1_Handler(unsigned long data);
  71. static void TIM2_Handler(unsigned long data);


  74. /*******************************************************************************
  75. ********************************************************************************
  76.                                                                         GPIO操作
  77. ********************************************************************************
  78. ********************************************************************************/
  79. void GPIO_ConfigPin(PORT port,unsigned int pin,PIN_MODE mode)
  80. {
  81.         PIO->Pn[port].CFG[pin / 8] &= ~((unsigned int)0x07 << pin % 8 * 4);
  82.         PIO->Pn[port].CFG[pin / 8] |=  ((unsigned int)mode << pin % 8 * 4);
  83. }

  85. void GPIO_SetPin(PORT port,unsigned int pin,unsigned int level)
  86. {
  87.         if(level)
  88.                 PIO->Pn[port].DAT |= (1 << pin);
  89.         else
  90.                 PIO->Pn[port].DAT &= ~(1 << pin);
  91. }

  93. unsigned int GPIO_GetPin(PORT port,unsigned int pin)
  94. {
  95.         if(PIO->Pn[port].DAT & (1 << pin))
  96.                 return 1;
  97.         else
  98.                 return 0;
  99. }



  103. /*******************************************************************************
  104. ********************************************************************************
  105.                                                                         驱动接口
  106. ********************************************************************************
  107. ********************************************************************************/
  108. static int device_open(struct inode *inode, struct file *filp)
  109. {
  110.         // printk("coder_openrn");
  111.         //创建内核定时器
  112.         setup_timer(&Timer1, TIM1_Handler, 0);
  113.         Timer1.expires = jiffies + TIMER1_INTERVAL;
  114.        
  115.         setup_timer(&Timer2, TIM2_Handler, 0);
  116.         Timer2.expires = jiffies + TIMER2_INTERVAL;
  117.        
  118.         add_timer(&Timer1);
  119.         add_timer(&Timer2);
  120.        
  121.         //映射物理内存到虚拟内存
  122.     PIO = (PIO_Map *)ioremap(PHYSICS_REG_ADDR, 0x200);
  123.         printk("VirtualAddr:%drn",(unsigned int)PIO);
  124.         GPIO_ConfigPin(PA,14,IN);
  125.         GPIO_ConfigPin(PA,16,IN);
  126.         LeftCoderPin = LeftCoderPin_Pre = GPIO_GetPin(PA,14);
  127.         RightCoderPin = RightCoderPin_Pre = GPIO_GetPin(PA,16);
  128.        
  129.         return 0;
  130. }

  132. static int device_release(struct inode *inode, struct file *filp)
  133. {
  134.         // printk("coder_releasern");
  135.         del_timer(&Timer1);
  136.         del_timer(&Timer2);
  137.         if(PIO != NULL)
  138.                 iounmap((void *)PIO); //撤销映射关系
  139.         return 0;
  140. }

  142. static int device_write(struct file *file, const char *buffer,size_t count, loff_t *ppos)
  143. {
  144.         // printk("coder_write:%drn",(int)count);
  145.         return count;
  146. }

  148. static int device_read(struct file *file, char *buffer,size_t count, loff_t *ppos)
  149. {
  150.         char str[2];
  151.         // printk("coder_readrn");
  152.         str[0] = (char)LeftCoderSpeed;
  153.         str[1] = (char)RightCoderSpeed;
  154.         copy_to_user(buffer, str, 2);
  155.     return 2;
  156. }

  158. int device_ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg)
  159. {
  160.     return 0;
  161. }
  162. static struct file_operations fops =
  163. {
  164.         .owner                        =        THIS_MODULE,
  165.         .compat_ioctl        =        device_ioctl,
  166.         .open                        =        device_open,
  167.         .read                        =        device_read,       
  168.         .write                        =        device_write,       
  169.         .release                =        device_release,
  170. };

  172. static void TIM1_Handler(unsigned long data)
  173. {
  174.         LeftCoderPin = GPIO_GetPin(PA,14);
  175.         RightCoderPin = GPIO_GetPin(PA,16);
  176.        
  177.         if(StateLeft == 0)
  178.         {
  179.                 if(LeftCoderPin != LeftCoderPin_Pre)
  180.                 {
  181.                         StateLeft = 1;
  182.                 }
  183.         }
  184.         else if(StateLeft == 1)
  185.         {
  186.                 LeftCoderCnt++;
  187.                 StateLeft = 0;
  188.         }
  189.        
  190.         if(StateRight == 0)
  191.         {
  192.                 if(RightCoderPin != RightCoderPin_Pre)
  193.                 {
  194.                         StateRight = 1;
  195.                 }
  196.         }
  197.         else if(StateRight == 1)
  198.         {
  199.                 RightCoderCnt++;
  200.                 StateRight = 0;
  201.         }
  202.        
  203.         LeftCoderPin_Pre        = LeftCoderPin;
  204.         RightCoderPin_Pre        = RightCoderPin;
  205.        
  206.         mod_timer(&Timer1, jiffies + TIMER1_INTERVAL);
  207. }

  209. static void TIM2_Handler(unsigned long data)
  210. {
  211.         LeftCoderSpeed = LeftCoderCnt;
  212.         LeftCoderCnt = 0;
  213.         RightCoderSpeed = RightCoderCnt;
  214.         RightCoderCnt = 0;
  215.         mod_timer(&Timer2, jiffies + TIMER2_INTERVAL);
  216. }

  218. static int __init dev_init(void)
  219. {
  220.         int result,err;
  221.         dev_t devno;
  222.        
  223.         printk("device_init:%drn",HZ);

  225.         devno = MKDEV(DEVICE_MAJOR, DEVICE_MINOR);
  226.         result = register_chrdev_region(devno, 1, DEVICE_NAME);
  227.         s_cdev = kmalloc(sizeof(struct cdev), GFP_KERNEL);
  228.         if (!s_cdev)
  229.         {
  230.                 result = -ENOMEM;
  231.                 goto fail_malloc;
  232.         }
  233.         cdev_init(s_cdev, &fops);
  234.         s_cdev->owner = THIS_MODULE;
  235.         err = cdev_add(s_cdev, devno, 1);
  236.        
  237.         struct class *dev_class = class_create(THIS_MODULE,DEVICE_NAME);
  238.         device_create(dev_class,NULL,devno,NULL,"zzz");
  239.        
  240.         return 0;
  241.        
  242. fail_malloc:
  243.         unregister_chrdev_region(devno, 1);
  244.         return result;
  245. }

  247. static void __exit dev_exit(void)
  248. {
  249.     cdev_del(s_cdev);
  250.     kfree(s_cdev);
  251.     unregister_chrdev(DEVICE_MAJOR, DEVICE_NAME);
  252.         printk("device_exit:%drn",__LINE__);
  253. }

  255. module_init(dev_init); //向Linux系统记录设备初始化的函数名称
  256. module_exit(dev_exit); //向Linux系统记录设备退出的函数名称

  258. MODULE_LICENSE("GPL");
  259. MODULE_AUTHOR("tangquan");
  260. MODULE_DESCRIPTION("motor coder driver!");




回帖（1）

小萃米

2017-1-18 09:35:31
很棒的分享啊
举报

更多回帖

1 0 1

相关帖子

发烧友 智能
发帖
电子发烧友APP
|投诉反馈|电子发烧友网
© 2021 bbs.elecfans.com
湘ICP备2023018690号
×
20
完善资料，
赚取积分