Chinaunix首页 | 论坛 | 博客

飕蔞髴澌acer.blog.chinaunix.net

首页　| 　博文目录　| 　关于我

soloforce

  • 博客访问： 311607
  • 博文数量： 27
  • 博客积分： 758
  • 博客等级： 军士长
  • 技术积分： 369
  • 用 户 组： 普通用户
  • 注册时间： 2010-04-08 23:10
文章分类

全部博文（27）

文章存档

2014年（1）

2012年（26）

我的朋友
最近访客
推荐博文
相关博文
自制Arduino小车

分类： 嵌入式

2012-07-04 23:27:24

硬件组成：
控制端：Arduino UNO主控板+蓝牙主机模块+摇杆模块+9V电池组
小车端：Arduino MEGA2560主控板+蓝牙从机模块+四驱小车支架（含四个减速直流电动机）+四路直流电机驱动模块+7.4V锂电池

图片如下：

软件代码：
控制端：

  1. #include "SoftwareSerial.h"


  4. #define BLUETOOTH_RX_DPIN 2
  5. #define BLUETOOTH_TX_DPIN 3
  6. #define JOYSTICK_KEY_DPIN 4
  7. #define JOYSTICK_X_APIN A0
  8. #define JOYSTICK_Y_APIN A1

  10. #define BLUETOOTH_BAUD_RATE 115200

  12. #define MANUAL_MODE 0
  13. #define AUTO_MODE 1
  14. int current_mode;

  16. SoftwareSerial bluetoothSerial(BLUETOOTH_RX_DPIN,BLUETOOTH_TX_DPIN);
  17. int oriX,oriY;
  18. int minX,maxX,minY,maxY;

  20. void setup()
  21. {

  23.   Serial.begin(9600);

  25.   pinMode(JOYSTICK_KEY_DPIN, INPUT);
  26.   pinMode(BLUETOOTH_RX_DPIN, INPUT);
  27.   pinMode(BLUETOOTH_TX_DPIN, OUTPUT);
  28.   bluetoothSerial.begin(BLUETOOTH_BAUD_RATE);
  29.   oriX = 0;
  30.   oriY = 0;
  31.   
  32.   current_mode=MANUAL_MODE;
  33.   
  34. }

  36. void loop()
  37. {
  38.     int X,Y;
  39.     float rx, ry;
  40.     int key_down = digitalRead(JOYSTICK_KEY_DPIN);
  41.     if(key_down == 0)
  42.     {
  43.       delay(50);
  44.       key_down = digitalRead(JOYSTICK_KEY_DPIN);
  45.       if(key_down==0){
  46.           bluetoothSerial.write('m');
  47.           current_mode=!current_mode;
  48.       }
  49.     }
  50.     

  52.    int sensorValue = analogRead(JOYSTICK_X_APIN);
  53.    if(oriX == 0)
  54.    {
  55.      oriX = sensorValue;
  56.      minX=0-oriX;
  57.      maxX=1023-oriX;
  58.    }
  59.   
  60.     X=sensorValue - oriX;
  61.    

  63.   
  64.    sensorValue = analogRead(JOYSTICK_Y_APIN);
  65.    if(oriY == 0)
  66.    {
  67.      oriY = sensorValue;
  68.      minY=0-oriY;
  69.      maxY=1023-oriY;
  70.    }
  71.    
  72.    Y=sensorValue - oriY;
  73.    
  74.    if(X>0) rx=(float)X/(float)maxX;
  75.    else if(X<0) rx=(float)X/(float)minX;
  76.    
  77.    if(Y>0) ry=(float)Y/(float)maxY;
  78.    else if(Y<0) ry=(float)Y/(float)minY;
  79.    
  80.     if(current_mode==MANUAL_MODE){
  81.       if(abs(X)<8 && abs(Y)<8) bluetoothSerial.write('h');
  82.       else if(Y>100 && abs(X)<50) bluetoothSerial.write('w');
  83.       else if(Y<-100 && abs(X)<50 ) bluetoothSerial.write('s');
  84.       else if(X>100 ) bluetoothSerial.write('d');
  85.       else if(X<-100 ) bluetoothSerial.write('a');
  86.     }
  87.     /*
  88.      if( rx <0.2 && ry <0.2 ) bluetoothSerial.write('h');
  89.      else if( ry>0.6 && rx<0.2){
  90.         if(Y<0) bluetoothSerial.write('s');
  91.         else if(Y>0) bluetoothSerial.write('w');
  92.      }else if(rx>0.6 && ry<0.5 ){
  93.        if(X<0 ) bluetoothSerial.write('a');
  94.        else if(X>0 ) bluetoothSerial.write('d');
  95.      }
  96.      */
  97.  
  98.    delay(50);

  100. }

小车端：

  1. #include "Arduino.h"
  2. #include "Servo.h"
  3. #include "SoftwareSerial.h"



  7. /*
  8.  Pins for PWM signal
  9. */
  10. #define MOTOR1_PWM 4
  11. #define MOTOR2_PWM 5
  12. #define MOTOR3_PWM 6
  13. #define MOTOR4_PWM 7

  15. /* camera 2-degree servos */
  16. #define SERVO_HORI 8
  17. #define SERVO_VERT 9

  19. /* ultra-sonic sensor */
  20. #define TRIG_PIN 10 //Trig_pin
  21. #define ECHO_PIN 11 //Echo_pin


  24. #define MAX_BUF_LEN 256

  26. #define WHEEL_MAX_SPEED 240
  27. #define WHEEL_TURN_SPEED 160


  30. #define SERVO_HORI_DEFAULT_DEGREE 80
  31. #define SERVO_VERT_DEFAULT_DEGREE 0
  32. #define SERVO_INCREMENT 10

  34. /*
  35.   Pins for logic input
  36. */
  37. #define MOTOR1_IN1 22
  38. #define MOTOR1_IN2 23
  39. #define MOTOR2_IN1 24
  40. #define MOTOR2_IN2 25
  41. #define MOTOR3_IN1 26
  42. #define MOTOR3_IN2 27
  43. #define MOTOR4_IN1 28
  44. #define MOTOR4_IN2 29

  46. #define SERIAL_BAUD 115200


  49. #define STATUS_STOP 0
  50. #define STATUS_FORWARD 1
  51. #define STATUS_BACKWARD 2
  52. #define STATUS_TURN_FORWARD_LEFT 3
  53. #define STATUS_TURN_FORWARD_RIGHT 4
  54. #define STATUS_TURN_BACKWARD_LEFT 5
  55. #define STATUS_TURN_BACKWARD_RIGHT 6

  57. #define MANUAL_MODE 0
  58. #define AUTO_MODE 1
  59. int current_mode;

  61. /*
  62.   speed (PWM relative) values for 4 wheel-motors
  63. */
  64. int wheel_forward_speed[4];
  65. int wheel_backward_speed[4];
  66. int wheel_turn_left_speed[4];
  67. int wheel_turn_right_speed[4];

  69. /* current car status */
  70. int current_status;

  72. Servo servo_hori, servo_vert;

  74. void wheel_id_to_pwm_pin(int wheel_id, int* pin)
  75. {
  76.   
  77.   switch(wheel_id){
  78.   case 1:
  79.     *pin=MOTOR1_PWM;
  80.     break;
  81.   case 2:
  82.     *pin=MOTOR2_PWM;
  83.     break;
  84.   case 3:
  85.     *pin=MOTOR3_PWM;
  86.     break;
  87.   case 4:
  88.     *pin=MOTOR4_PWM;
  89.     break;
  90.   default:
  91.     Serial.println("illegal wheel_id input\n");
  92.   }
  93. }

  95. void wheel_id_to_logic_pin(int wheel_id, int* in1, int* in2)
  96. {
  97.   switch(wheel_id){
  98.   case 1:
  99.     *in1=MOTOR1_IN1;
  100.     *in2=MOTOR1_IN2;
  101.     break;
  102.   case 2:
  103.     *in1=MOTOR2_IN1;
  104.     *in2=MOTOR2_IN2;
  105.     break;
  106.   case 3:
  107.     *in1=MOTOR3_IN1;
  108.     *in2=MOTOR3_IN2;
  109.     break;
  110.   case 4:
  111.     *in1=MOTOR4_IN1;
  112.     *in2=MOTOR4_IN2;
  113.     break;
  114.   default:
  115.     Serial.println("illegal wheel_id input\n");
  116.   }
  117. }



  121. void wheel_driver(int wheel_id, int wheel_speed)
  122. {
  123.   int pwm_pin;
  124.   int in1,in2;
  125.   int wheel_dir=0;
  126.   
  127.   if(wheel_id<1 || wheel_id>4) return; // do nothing
  128.   
  129.   
  130.   wheel_id_to_pwm_pin(wheel_id, &pwm_pin);
  131.   wheel_id_to_logic_pin(wheel_id, &in1, &in2);
  132.   
  133.   // set speed parameter
  134.   if(wheel_speed<0){
  135.     wheel_dir=-1;
  136.     wheel_speed=-wheel_speed;
  137.   }else{
  138.     wheel_dir=1;
  139.   }
  140.  
  141.   if(wheel_speed>255) wheel_speed=255;
  142.   
  143.   analogWrite(pwm_pin, wheel_speed);
  144.       
  145.   // stop the motor if speed is 0
  146.   if(wheel_speed==0){
  147.     digitalWrite(in1, LOW);
  148.     digitalWrite(in2, LOW);
  149.   }
  150.   
  151.   
  152.   if(wheel_dir>0){
  153.     // make wheel scroll forward
  154.     digitalWrite(in1, HIGH);
  155.     digitalWrite(in2, LOW);
  156.   }else if(wheel_dir<0){
  157.     // scroll backward
  158.     digitalWrite(in1, LOW);
  159.     digitalWrite(in2, HIGH);
  160.   }else{
  161.     // stop the wheel
  162.     digitalWrite(in1, LOW);
  163.     digitalWrite(in2, LOW);
  164.     analogWrite(pwm_pin,0);
  165.   }
  166.   
  167. }



  171. void car_stop()
  172. {
  173.   int i;
  174.   if(current_status==STATUS_STOP) return;
  175.   for(i=1;i<=4;i++){
  176.     wheel_driver(i, 0);
  177.   }
  178.   current_status=STATUS_STOP;
  179. }

  181. void car_forward()
  182. {
  183.   int i;
  184.   if(current_status==STATUS_FORWARD) return;
  185.   car_stop();
  186.   for(i=1;i<=4;i++){
  187.     wheel_driver(i, wheel_forward_speed[i-1]);
  188.   }
  189.   current_status=STATUS_FORWARD;
  190. }

  192. void car_backward()
  193. {
  194.   int i;
  195.   if(current_status==STATUS_BACKWARD) return;
  196.   car_stop();
  197.   for(i=1;i<=4;i++){
  198.     wheel_driver(i, wheel_backward_speed[i-1]);
  199.   }
  200.   current_status=STATUS_BACKWARD;
  201. }

  203. void car_turn_forward_left()
  204. {
  205.   int i;
  206.   if(current_status==STATUS_TURN_FORWARD_LEFT) return;
  207.   car_stop();
  208.   for(i=1;i<=4;i++){
  209.     wheel_driver(i, wheel_turn_left_speed[i-1]);
  210.   }
  211.   
  212.   current_status=STATUS_TURN_FORWARD_LEFT;
  213. }


  216. void car_turn_forward_right()
  217. {
  218.   int i;
  219.   if(current_status==STATUS_TURN_FORWARD_RIGHT) return;
  220.   car_stop();
  221.   for(i=1;i<=4;i++){
  222.     wheel_driver(i, wheel_turn_right_speed[i-1]);
  223.   }
  224.   current_status=STATUS_TURN_FORWARD_RIGHT;
  225. }



  229. void process_cmd(char* cmd, int cmd_len)
  230. {
  231.   int i;
  232.   int cmd_step=1;
  233.   int h_degree, v_degree;
  234.   
  235.   if(servo_hori.attached()) h_degree=servo_hori.read();
  236.   if(servo_hori.attached()) v_degree=servo_vert.read();
  237.   
  238.   for(i=0;i<cmd_len;i+=cmd_step){
  239.     switch(cmd[i]){
  240.     case 'w':
  241.       car_forward();
  242.       break;
  243.     case 's':
  244.       car_backward();
  245.       break;
  246.     case 'a':
  247.       car_turn_forward_left();
  248.       break;
  249.     case 'd':
  250.       car_turn_forward_right();
  251.       break;
  252.     case 'm':
  253.       if(current_mode==MANUAL_MODE) current_mode=AUTO_MODE;
  254.       else current_mode=MANUAL_MODE;
  255.       break;
  256.     case 'h':
  257.       car_stop();
  258.       break;
  259.     case 'j':
  260.       if(servo_hori.attached())
  261.         servo_hori.write(h_degree-SERVO_INCREMENT);
  262.       delay(50);
  263.       break;
  264.     case 'l':
  265.       if(servo_hori.attached())
  266.         servo_hori.write(h_degree+SERVO_INCREMENT);
  267.       delay(50);
  268.       break;
  269.     case 'i':
  270.       if(servo_vert.attached())
  271.         servo_vert.write(v_degree-SERVO_INCREMENT);
  272.       delay(50);
  273.       break;
  274.     case 'k':
  275.       if(servo_vert.attached())
  276.         servo_vert.write(v_degree+SERVO_INCREMENT);
  277.       delay(50);
  278.       break;
  279.     default:
  280.       break;
  281.     }
  282.   }

  284. }


  287. int Distance(long time)
  288. {
  289.     // Distance_CM = ((Duration of high level)*(Sonic :340m/s))/2
  290.     // = ((Duration of high level)*(Sonic :0.034 cm/us))/2
  291.     // = ((Duration of high level)/(Sonic :29.4 cm/us))/2
  292.     int dist = time/29/2 ;
  293.   
  294.     return dist;
  295. }

  297. long Trig_Echo()
  298. {
  299.   digitalWrite(TRIG_PIN, LOW);
  300.   delayMicroseconds(2);
  301.   digitalWrite(TRIG_PIN, HIGH); // pull the Trig pin to high level for more than 10us impulse
  302.   delayMicroseconds(10);
  303.   digitalWrite(TRIG_PIN, LOW);
  304.   long microseconds = pulseIn(ECHO_PIN,HIGH); // waits for the pin to go HIGH, and returns the length of the pulse in microseconds
  305.   return microseconds; // return microseconds
  306. }

  308. void setup()
  309. {
  310.   
  311.   /*
  312.   servo_hori.attach(SERVO_HORI);
  313.   servo_vert.attach(SERVO_VERT);
  314.   servo_hori.write(SERVO_HORI_DEFAULT_DEGREE);
  315.   servo_vert.write(SERVO_VERT_DEFAULT_DEGREE);
  316.   */
  317.   
  318.   Serial.begin(SERIAL_BAUD); // init serial
  319.   
  320.   pinMode(MOTOR1_IN1,OUTPUT);
  321.   pinMode(MOTOR1_IN2,OUTPUT);
  322.   pinMode(MOTOR2_IN1,OUTPUT);
  323.   pinMode(MOTOR2_IN2,OUTPUT);
  324.   pinMode(MOTOR3_IN1,OUTPUT);
  325.   pinMode(MOTOR3_IN2,OUTPUT);
  326.   pinMode(MOTOR4_IN1,OUTPUT);
  327.   pinMode(MOTOR4_IN2,OUTPUT);
  328.   
  329.   pinMode(TRIG_PIN,OUTPUT); // set output pin for trigger
  330.   pinMode(ECHO_PIN,INPUT); // set input pin for echo
  331.   
  332.   
  333.   /* setting default wheel speeds */
  334.   int i;
  335.   for(i=0;i<4;i++){
  336.     wheel_forward_speed[i]=WHEEL_MAX_SPEED;
  337.     wheel_backward_speed[i]=-WHEEL_MAX_SPEED;
  338.   }
  339.   
  340.   wheel_turn_left_speed[0]=WHEEL_TURN_SPEED;
  341.   wheel_turn_left_speed[1]=WHEEL_TURN_SPEED;
  342.   wheel_turn_left_speed[2]=-WHEEL_TURN_SPEED;
  343.   wheel_turn_left_speed[3]=-WHEEL_TURN_SPEED;
  344.   
  345.   wheel_turn_right_speed[0]=-WHEEL_TURN_SPEED;
  346.   wheel_turn_right_speed[1]=-WHEEL_TURN_SPEED;
  347.   wheel_turn_right_speed[2]=WHEEL_TURN_SPEED;
  348.   wheel_turn_right_speed[3]=WHEEL_TURN_SPEED;
  349.   
  350.   
  351.   /* make the car stop */
  352.   car_stop();
  353.   
  354.   current_mode=MANUAL_MODE;
  355. }




  360. void loop()
  361. {
  362.   int i,cmd_len;
  363.   char buf[MAX_BUF_LEN]={0};
  364.   char ch;
  365.   int wheel_id, wheel_speed;
  366.   if(Serial.available()){
  367.     i=0;
  368.     do{
  369.       buf[i++] = Serial.read();
  370.       delay(2);
  371.     }while(Serial.available() && i<MAX_BUF_LEN);
  372.     process_cmd(buf, i);
  373.   }
  374.   
  375.   if(current_mode==AUTO_MODE){
  376.     long microseconds = Trig_Echo();
  377.     int dist_cm = Distance(microseconds);
  378.     while(dist_cm<30 && dist_cm>0 ) {
  379.       car_turn_forward_right();
  380.       microseconds = Trig_Echo();
  381.       dist_cm = Distance(microseconds);
  382.     }
  383.     car_forward();
  384.   }
  385. }



阅读(9992) | 评论(0) | 转发(0) |
0

上一篇：Lighttpd+PHP

下一篇：[转] 魅族MX老双核(M030) Flyme 1.01 老双核的UART破解尝试

给主人留下些什么吧！~~
关于我们 | 关于IT168 | 联系方式 | 广告合作 | 法律声明 | 免费注册

Copyright 2001-2010 ChinaUnix.net All Rights Reserved 北京皓辰网域网络信息技术有限公司. 版权所有

感谢所有关心和支持过ChinaUnix的朋友们

16024965号-6