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

全部博文(27)

文章存档

2014年(1)

2012年(26)

我的朋友

分类: 嵌入式

2012-07-04 23:27:24

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

图片如下:

软件代码:
控制端:

  1. #include "SoftwareSerial.h"


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

  7. #define BLUETOOTH_BAUD_RATE 115200

  8. #define MANUAL_MODE 0
  9. #define AUTO_MODE 1
  10. int current_mode;

  11. SoftwareSerial bluetoothSerial(BLUETOOTH_RX_DPIN,BLUETOOTH_TX_DPIN);
  12. int oriX,oriY;
  13. int minX,maxX,minY,maxY;

  14. void setup()
  15. {

  16.   Serial.begin(9600);

  17.   pinMode(JOYSTICK_KEY_DPIN, INPUT);
  18.   pinMode(BLUETOOTH_RX_DPIN, INPUT);
  19.   pinMode(BLUETOOTH_TX_DPIN, OUTPUT);
  20.   bluetoothSerial.begin(BLUETOOTH_BAUD_RATE);
  21.   oriX = 0;
  22.   oriY = 0;
  23.   
  24.   current_mode=MANUAL_MODE;
  25.   
  26. }

  27. void loop()
  28. {
  29.     int X,Y;
  30.     float rx, ry;
  31.     int key_down = digitalRead(JOYSTICK_KEY_DPIN);
  32.     if(key_down == 0)
  33.     {
  34.       delay(50);
  35.       key_down = digitalRead(JOYSTICK_KEY_DPIN);
  36.       if(key_down==0){
  37.           bluetoothSerial.write('m');
  38.           current_mode=!current_mode;
  39.       }
  40.     }
  41.     

  42.    int sensorValue = analogRead(JOYSTICK_X_APIN);
  43.    if(oriX == 0)
  44.    {
  45.      oriX = sensorValue;
  46.      minX=0-oriX;
  47.      maxX=1023-oriX;
  48.    }
  49.   
  50.     X=sensorValue - oriX;
  51.    

  52.   
  53.    sensorValue = analogRead(JOYSTICK_Y_APIN);
  54.    if(oriY == 0)
  55.    {
  56.      oriY = sensorValue;
  57.      minY=0-oriY;
  58.      maxY=1023-oriY;
  59.    }
  60.    
  61.    Y=sensorValue - oriY;
  62.    
  63.    if(X>0) rx=(float)X/(float)maxX;
  64.    else if(X<0) rx=(float)X/(float)minX;
  65.    
  66.    if(Y>0) ry=(float)Y/(float)maxY;
  67.    else if(Y<0) ry=(float)Y/(float)minY;
  68.    
  69.     if(current_mode==MANUAL_MODE){
  70.       if(abs(X)<8 && abs(Y)<8) bluetoothSerial.write('h');
  71.       else if(Y>100 && abs(X)<50) bluetoothSerial.write('w');
  72.       else if(Y<-100 && abs(X)<50 ) bluetoothSerial.write('s');
  73.       else if(X>100 ) bluetoothSerial.write('d');
  74.       else if(X<-100 ) bluetoothSerial.write('a');
  75.     }
  76.     /*
  77.      if( rx <0.2 && ry <0.2 ) bluetoothSerial.write('h');
  78.      else if( ry>0.6 && rx<0.2){
  79.         if(Y<0) bluetoothSerial.write('s');
  80.         else if(Y>0) bluetoothSerial.write('w');
  81.      }else if(rx>0.6 && ry<0.5 ){
  82.        if(X<0 ) bluetoothSerial.write('a');
  83.        else if(X>0 ) bluetoothSerial.write('d');
  84.      }
  85.      */
  86.  
  87.    delay(50);

  88. }

小车端:

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



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

  11. /* camera 2-degree servos */
  12. #define SERVO_HORI 8
  13. #define SERVO_VERT 9

  14. /* ultra-sonic sensor */
  15. #define TRIG_PIN 10 //Trig_pin
  16. #define ECHO_PIN 11 //Echo_pin


  17. #define MAX_BUF_LEN 256

  18. #define WHEEL_MAX_SPEED 240
  19. #define WHEEL_TURN_SPEED 160


  20. #define SERVO_HORI_DEFAULT_DEGREE 80
  21. #define SERVO_VERT_DEFAULT_DEGREE 0
  22. #define SERVO_INCREMENT 10

  23. /*
  24.   Pins for logic input
  25. */
  26. #define MOTOR1_IN1 22
  27. #define MOTOR1_IN2 23
  28. #define MOTOR2_IN1 24
  29. #define MOTOR2_IN2 25
  30. #define MOTOR3_IN1 26
  31. #define MOTOR3_IN2 27
  32. #define MOTOR4_IN1 28
  33. #define MOTOR4_IN2 29

  34. #define SERIAL_BAUD 115200


  35. #define STATUS_STOP 0
  36. #define STATUS_FORWARD 1
  37. #define STATUS_BACKWARD 2
  38. #define STATUS_TURN_FORWARD_LEFT 3
  39. #define STATUS_TURN_FORWARD_RIGHT 4
  40. #define STATUS_TURN_BACKWARD_LEFT 5
  41. #define STATUS_TURN_BACKWARD_RIGHT 6

  42. #define MANUAL_MODE 0
  43. #define AUTO_MODE 1
  44. int current_mode;

  45. /*
  46.   speed (PWM relative) values for 4 wheel-motors
  47. */
  48. int wheel_forward_speed[4];
  49. int wheel_backward_speed[4];
  50. int wheel_turn_left_speed[4];
  51. int wheel_turn_right_speed[4];

  52. /* current car status */
  53. int current_status;

  54. Servo servo_hori, servo_vert;

  55. void wheel_id_to_pwm_pin(int wheel_id, int* pin)
  56. {
  57.   
  58.   switch(wheel_id){
  59.   case 1:
  60.     *pin=MOTOR1_PWM;
  61.     break;
  62.   case 2:
  63.     *pin=MOTOR2_PWM;
  64.     break;
  65.   case 3:
  66.     *pin=MOTOR3_PWM;
  67.     break;
  68.   case 4:
  69.     *pin=MOTOR4_PWM;
  70.     break;
  71.   default:
  72.     Serial.println("illegal wheel_id input\n");
  73.   }
  74. }

  75. void wheel_id_to_logic_pin(int wheel_id, int* in1, int* in2)
  76. {
  77.   switch(wheel_id){
  78.   case 1:
  79.     *in1=MOTOR1_IN1;
  80.     *in2=MOTOR1_IN2;
  81.     break;
  82.   case 2:
  83.     *in1=MOTOR2_IN1;
  84.     *in2=MOTOR2_IN2;
  85.     break;
  86.   case 3:
  87.     *in1=MOTOR3_IN1;
  88.     *in2=MOTOR3_IN2;
  89.     break;
  90.   case 4:
  91.     *in1=MOTOR4_IN1;
  92.     *in2=MOTOR4_IN2;
  93.     break;
  94.   default:
  95.     Serial.println("illegal wheel_id input\n");
  96.   }
  97. }



  98. void wheel_driver(int wheel_id, int wheel_speed)
  99. {
  100.   int pwm_pin;
  101.   int in1,in2;
  102.   int wheel_dir=0;
  103.   
  104.   if(wheel_id<1 || wheel_id>4) return; // do nothing
  105.   
  106.   
  107.   wheel_id_to_pwm_pin(wheel_id, &pwm_pin);
  108.   wheel_id_to_logic_pin(wheel_id, &in1, &in2);
  109.   
  110.   // set speed parameter
  111.   if(wheel_speed<0){
  112.     wheel_dir=-1;
  113.     wheel_speed=-wheel_speed;
  114.   }else{
  115.     wheel_dir=1;
  116.   }
  117.  
  118.   if(wheel_speed>255) wheel_speed=255;
  119.   
  120.   analogWrite(pwm_pin, wheel_speed);
  121.       
  122.   // stop the motor if speed is 0
  123.   if(wheel_speed==0){
  124.     digitalWrite(in1, LOW);
  125.     digitalWrite(in2, LOW);
  126.   }
  127.   
  128.   
  129.   if(wheel_dir>0){
  130.     // make wheel scroll forward
  131.     digitalWrite(in1, HIGH);
  132.     digitalWrite(in2, LOW);
  133.   }else if(wheel_dir<0){
  134.     // scroll backward
  135.     digitalWrite(in1, LOW);
  136.     digitalWrite(in2, HIGH);
  137.   }else{
  138.     // stop the wheel
  139.     digitalWrite(in1, LOW);
  140.     digitalWrite(in2, LOW);
  141.     analogWrite(pwm_pin,0);
  142.   }
  143.   
  144. }



  145. void car_stop()
  146. {
  147.   int i;
  148.   if(current_status==STATUS_STOP) return;
  149.   for(i=1;i<=4;i++){
  150.     wheel_driver(i, 0);
  151.   }
  152.   current_status=STATUS_STOP;
  153. }

  154. void car_forward()
  155. {
  156.   int i;
  157.   if(current_status==STATUS_FORWARD) return;
  158.   car_stop();
  159.   for(i=1;i<=4;i++){
  160.     wheel_driver(i, wheel_forward_speed[i-1]);
  161.   }
  162.   current_status=STATUS_FORWARD;
  163. }

  164. void car_backward()
  165. {
  166.   int i;
  167.   if(current_status==STATUS_BACKWARD) return;
  168.   car_stop();
  169.   for(i=1;i<=4;i++){
  170.     wheel_driver(i, wheel_backward_speed[i-1]);
  171.   }
  172.   current_status=STATUS_BACKWARD;
  173. }

  174. void car_turn_forward_left()
  175. {
  176.   int i;
  177.   if(current_status==STATUS_TURN_FORWARD_LEFT) return;
  178.   car_stop();
  179.   for(i=1;i<=4;i++){
  180.     wheel_driver(i, wheel_turn_left_speed[i-1]);
  181.   }
  182.   
  183.   current_status=STATUS_TURN_FORWARD_LEFT;
  184. }


  185. void car_turn_forward_right()
  186. {
  187.   int i;
  188.   if(current_status==STATUS_TURN_FORWARD_RIGHT) return;
  189.   car_stop();
  190.   for(i=1;i<=4;i++){
  191.     wheel_driver(i, wheel_turn_right_speed[i-1]);
  192.   }
  193.   current_status=STATUS_TURN_FORWARD_RIGHT;
  194. }



  195. void process_cmd(char* cmd, int cmd_len)
  196. {
  197.   int i;
  198.   int cmd_step=1;
  199.   int h_degree, v_degree;
  200.   
  201.   if(servo_hori.attached()) h_degree=servo_hori.read();
  202.   if(servo_hori.attached()) v_degree=servo_vert.read();
  203.   
  204.   for(i=0;i<cmd_len;i+=cmd_step){
  205.     switch(cmd[i]){
  206.     case 'w':
  207.       car_forward();
  208.       break;
  209.     case 's':
  210.       car_backward();
  211.       break;
  212.     case 'a':
  213.       car_turn_forward_left();
  214.       break;
  215.     case 'd':
  216.       car_turn_forward_right();
  217.       break;
  218.     case 'm':
  219.       if(current_mode==MANUAL_MODE) current_mode=AUTO_MODE;
  220.       else current_mode=MANUAL_MODE;
  221.       break;
  222.     case 'h':
  223.       car_stop();
  224.       break;
  225.     case 'j':
  226.       if(servo_hori.attached())
  227.         servo_hori.write(h_degree-SERVO_INCREMENT);
  228.       delay(50);
  229.       break;
  230.     case 'l':
  231.       if(servo_hori.attached())
  232.         servo_hori.write(h_degree+SERVO_INCREMENT);
  233.       delay(50);
  234.       break;
  235.     case 'i':
  236.       if(servo_vert.attached())
  237.         servo_vert.write(v_degree-SERVO_INCREMENT);
  238.       delay(50);
  239.       break;
  240.     case 'k':
  241.       if(servo_vert.attached())
  242.         servo_vert.write(v_degree+SERVO_INCREMENT);
  243.       delay(50);
  244.       break;
  245.     default:
  246.       break;
  247.     }
  248.   }

  249. }


  250. int Distance(long time)
  251. {
  252.     // Distance_CM = ((Duration of high level)*(Sonic :340m/s))/2
  253.     // = ((Duration of high level)*(Sonic :0.034 cm/us))/2
  254.     // = ((Duration of high level)/(Sonic :29.4 cm/us))/2
  255.     int dist = time/29/2 ;
  256.   
  257.     return dist;
  258. }

  259. long Trig_Echo()
  260. {
  261.   digitalWrite(TRIG_PIN, LOW);
  262.   delayMicroseconds(2);
  263.   digitalWrite(TRIG_PIN, HIGH); // pull the Trig pin to high level for more than 10us impulse
  264.   delayMicroseconds(10);
  265.   digitalWrite(TRIG_PIN, LOW);
  266.   long microseconds = pulseIn(ECHO_PIN,HIGH); // waits for the pin to go HIGH, and returns the length of the pulse in microseconds
  267.   return microseconds; // return microseconds
  268. }

  269. void setup()
  270. {
  271.   
  272.   /*
  273.   servo_hori.attach(SERVO_HORI);
  274.   servo_vert.attach(SERVO_VERT);
  275.   servo_hori.write(SERVO_HORI_DEFAULT_DEGREE);
  276.   servo_vert.write(SERVO_VERT_DEFAULT_DEGREE);
  277.   */
  278.   
  279.   Serial.begin(SERIAL_BAUD); // init serial
  280.   
  281.   pinMode(MOTOR1_IN1,OUTPUT);
  282.   pinMode(MOTOR1_IN2,OUTPUT);
  283.   pinMode(MOTOR2_IN1,OUTPUT);
  284.   pinMode(MOTOR2_IN2,OUTPUT);
  285.   pinMode(MOTOR3_IN1,OUTPUT);
  286.   pinMode(MOTOR3_IN2,OUTPUT);
  287.   pinMode(MOTOR4_IN1,OUTPUT);
  288.   pinMode(MOTOR4_IN2,OUTPUT);
  289.   
  290.   pinMode(TRIG_PIN,OUTPUT); // set output pin for trigger
  291.   pinMode(ECHO_PIN,INPUT); // set input pin for echo
  292.   
  293.   
  294.   /* setting default wheel speeds */
  295.   int i;
  296.   for(i=0;i<4;i++){
  297.     wheel_forward_speed[i]=WHEEL_MAX_SPEED;
  298.     wheel_backward_speed[i]=-WHEEL_MAX_SPEED;
  299.   }
  300.   
  301.   wheel_turn_left_speed[0]=WHEEL_TURN_SPEED;
  302.   wheel_turn_left_speed[1]=WHEEL_TURN_SPEED;
  303.   wheel_turn_left_speed[2]=-WHEEL_TURN_SPEED;
  304.   wheel_turn_left_speed[3]=-WHEEL_TURN_SPEED;
  305.   
  306.   wheel_turn_right_speed[0]=-WHEEL_TURN_SPEED;
  307.   wheel_turn_right_speed[1]=-WHEEL_TURN_SPEED;
  308.   wheel_turn_right_speed[2]=WHEEL_TURN_SPEED;
  309.   wheel_turn_right_speed[3]=WHEEL_TURN_SPEED;
  310.   
  311.   
  312.   /* make the car stop */
  313.   car_stop();
  314.   
  315.   current_mode=MANUAL_MODE;
  316. }




  317. void loop()
  318. {
  319.   int i,cmd_len;
  320.   char buf[MAX_BUF_LEN]={0};
  321.   char ch;
  322.   int wheel_id, wheel_speed;
  323.   if(Serial.available()){
  324.     i=0;
  325.     do{
  326.       buf[i++] = Serial.read();
  327.       delay(2);
  328.     }while(Serial.available() && i<MAX_BUF_LEN);
  329.     process_cmd(buf, i);
  330.   }
  331.   
  332.   if(current_mode==AUTO_MODE){
  333.     long microseconds = Trig_Echo();
  334.     int dist_cm = Distance(microseconds);
  335.     while(dist_cm<30 && dist_cm>0 ) {
  336.       car_turn_forward_right();
  337.       microseconds = Trig_Echo();
  338.       dist_cm = Distance(microseconds);
  339.     }
  340.     car_forward();
  341.   }
  342. }



阅读(10003) | 评论(0) | 转发(0) |
给主人留下些什么吧!~~