robot

z foreste, 1 rok temu, napisane w C++, wyświetlone 115 razy. [paste_expire] 1 sekunda.
URL https://codetab.e3x.pl/view/39d36de3 Udostępnij
Pobierz wklejkę lub Pokaż surowy tekst
  1. #include <mbed.h>
  2. #include "ArduinoMotorShield.hpp"
  3. #include "ultrasonic.h"
  4. #include "PinDetect.h"
  5. /*
  6. DigitalOut F_Left(D12);
  7. DigitalOut F_Right(D13);
  8. DigitalOut B_Left(D8);
  9. DigitalOut B_Right(D9);
  10. DigitalOut start_Left(D11);
  11. DigitalOut start_Right(D4);
  12. */
  13. Timer count , count2, coun3 ;
  14. ArduinoMotorShield engine;
  15. DigitalOut Buzzer(PB_12);
  16. AnalogIn sensor_light(PC_3);
  17. Serial Bluetooth(PC_4, PC_5);
  18. PinDetect left_sensor(PC_6);
  19. PinDetect right_sensor(PC_8);
  20. static int left_sensor_state = 0;
  21. static int right_sensor_state = 0;
  22. bool lock = true;
  23.  
  24. class Miernik
  25. {
  26.        
  27. public:
  28.         Miernik(PinName pin)
  29.                 : _analog(pin)
  30.         {
  31.         }
  32.         float xx()
  33.         {
  34.                 return _analog.read();
  35.         }
  36. private:
  37.         AnalogIn  _analog;
  38.        
  39. };
  40. class Led
  41. {
  42. public:
  43.         Led(PinName pin)
  44.                 : _pin(pin)
  45.         {
  46.                 _pin = 0;
  47.         }
  48.         void state(int s)
  49.         {
  50.                 _pin = s;
  51.         }
  52.         void toggle()
  53.         {
  54.                 _pin = !_pin;
  55.                
  56.         }
  57. protected:
  58.         DigitalOut _pin;
  59. };
  60.  
  61. Led Led_front_right(PB_15);
  62. Led Led_front_left(PB_1);
  63. Led Led_back_right(PB_2);
  64. Led Led_back_left(PB_11);
  65. Miernik pomiar(PC_3);
  66. void(*bips)();
  67.  
  68. void collision_sensor()
  69. {
  70.         if (right_sensor_state == 1)
  71.         {
  72.                 lock = false;
  73.                 coun3.start();
  74.                 engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  75.                 engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  76.                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  77.                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.3);
  78.                 if (coun3.read() >= 5)
  79.                 {
  80.                         right_sensor_state = 0;
  81.                         lock = true;
  82.                         coun3.stop();
  83.                         coun3.reset();
  84.                 }
  85.         }
  86.        
  87.         if (left_sensor_state == 1)
  88.         {
  89.                 lock = false;
  90.                 coun3.start();
  91.                 engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  92.                 engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  93.                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.3);
  94.                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
  95.                 if (coun3.read() >= 5)
  96.                 {
  97.                         left_sensor_state = 0;
  98.                         lock = true;
  99.                         coun3.stop();
  100.                         coun3.reset();
  101.                 }
  102.         }
  103. }
  104.  
  105. void dist(int distance)
  106.        
  107. {  
  108.         if (lock == true)
  109.         {
  110.                
  111.                 if ((distance <= 30) && (distance >= 0))
  112.                 {
  113.                         engine.SetBrake(ArduinoMotorShield::MOTOR_A, 1);
  114.                         engine.SetBrake(ArduinoMotorShield::MOTOR_B, 1);
  115.                         bips();
  116.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.0);
  117.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.0);
  118.                         count2.start();
  119.                         if (count2.read_ms() >= 3000)
  120.                         {
  121.                                 engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  122.                                 engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  123.                                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  124.                                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
  125.                                 if (count2.read_ms() >= 5000)
  126.                                 {
  127.                                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  128.                                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
  129.                                 }
  130.                        
  131.                                 if ((count2.read_ms() >= 7000) && (distance <= 50))
  132.                                 {
  133.                                         count2.stop();
  134.                                         count2.reset();
  135.                                 }
  136.                         }
  137.                 }
  138.                 else    if (distance <= 50)
  139.                 {
  140.                
  141.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.2);
  142.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.2);
  143.                 }
  144.                 else if (distance <= 60)
  145.                 {
  146.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.3);
  147.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.3);
  148.                 }
  149.                 else if (distance <= 70)
  150.                 {
  151.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.4);
  152.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.4);
  153.                 }
  154.                 else if (distance <= 80)
  155.                 {
  156.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.5);
  157.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.5);
  158.                 }
  159.                 else if (distance <= 90)
  160.                 {
  161.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.6);
  162.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
  163.                 }
  164.                 else if (distance <= 100)
  165.                 {
  166.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.7);
  167.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.7);
  168.                 }
  169.        
  170.                 else
  171.                 {
  172.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.8);
  173.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.8);
  174.                         engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  175.                         engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  176.                 }
  177.                 Bluetooth.printf("Distance %d mm\r\n", distance);
  178.         }
  179. }
  180.  
  181. void right_sensor_push()
  182. {
  183.         right_sensor_state = 1;
  184. }
  185.  
  186. void left_sensor_push()
  187. {
  188.         left_sensor_state = 1;
  189. }
  190. ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  191. void bip()
  192. {
  193.         count.start();
  194.         if (count.read_ms() >= 500)
  195.         {
  196.                 Led_back_left.toggle();
  197.                 Buzzer = !Buzzer;
  198.                 count.stop();
  199.                 count.reset();
  200.         }
  201. }
  202.  
  203.  
  204.  
  205. int main()
  206. {
  207.         engine.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
  208.         engine.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
  209.         bips = &bip;
  210.         left_sensor.mode(PullUp);
  211.         left_sensor.attach_deasserted( &left_sensor_push);
  212.         left_sensor.setSampleFrequency();
  213.        
  214.        
  215.         right_sensor.mode(PullUp);
  216.         right_sensor.attach_deasserted(&right_sensor_push);
  217.         right_sensor.setSampleFrequency();
  218.         sensor.startUpdates();
  219.         for (;;)
  220.         {
  221.                
  222.                 sensor.checkDistance();
  223.                 //bip();
  224.                 collision_sensor();
  225.                 Led_front_right.state(0);
  226.                 Led_front_left.state(0);
  227.                 Led_back_right.state(0);
  228.                 //Led_back_left.state(1);
  229.                
  230.                 Bluetooth.printf("This program runs since %1.2fv seconds.\n", pomiar.xx());
  231.                
  232.         }
  233. }

odpowiedź "robot"

Tutaj możesz odpowiedzieć na wklejkę z góry

captcha