robot

z foreste, 1 rok temu, napisane w C++, wyświetlone 113 razy. [paste_expire] 1 sekunda.
URL https://codetab.e3x.pl/view/9b303817 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. bool lock = true;
  22. /*
  23. DigitalOut Led_front_right(PB_15);
  24. DigitalOut Led_front_left(PB_1);
  25. DigitalOut Led_back_right(PB_2);
  26. DigitalOut Led_back_left(PB_11);
  27. */
  28. void(*bips)();
  29.  
  30. void collision_sensor()
  31. {
  32.         if (left_sensor_state == 1)
  33.         {
  34.                 lock = false;
  35.                 coun3.start();
  36.                 engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  37.                 engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  38.                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.1);
  39.                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
  40.                 if (coun3.read() >= 5)
  41.                 {
  42.                         left_sensor_state = 0;
  43.                         lock = true;
  44.                         coun3.stop();
  45.                         coun3.reset();
  46.                 }
  47.         }
  48. }
  49.  
  50. void dist(int distance)
  51.        
  52. {  
  53.         if (lock == true)
  54.         {
  55.                
  56.                 if ((distance <= 30) && (distance >= 0))
  57.                 {
  58.                         engine.SetBrake(ArduinoMotorShield::MOTOR_A, 1);
  59.                         engine.SetBrake(ArduinoMotorShield::MOTOR_B, 1);
  60.                         bips();
  61.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.0);
  62.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.0);
  63.                         count2.start();
  64.                         if (count2.read_ms() >= 3000)
  65.                         {
  66.                                 engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  67.                                 engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  68.                                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  69.                                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
  70.                                 if (count2.read_ms() >= 5000)
  71.                                 {
  72.                                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  73.                                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
  74.                                 }
  75.                        
  76.                                 if ((count2.read_ms() >= 7000) && (distance <= 50))
  77.                                 {
  78.                                         count2.stop();
  79.                                         count2.reset();
  80.                                 }
  81.                         }
  82.                 }
  83.                 else    if (distance <= 50)
  84.                 {
  85.                
  86.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.2);
  87.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.2);
  88.                 }
  89.                 else if (distance <= 60)
  90.                 {
  91.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.3);
  92.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.3);
  93.                 }
  94.                 else if (distance <= 70)
  95.                 {
  96.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.4);
  97.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.4);
  98.                 }
  99.                 else if (distance <= 80)
  100.                 {
  101.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.5);
  102.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.5);
  103.                 }
  104.                 else if (distance <= 90)
  105.                 {
  106.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.6);
  107.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
  108.                 }
  109.                 else if (distance <= 100)
  110.                 {
  111.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.7);
  112.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.7);
  113.                 }
  114.        
  115.                 else
  116.                 {
  117.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.8);
  118.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.8);
  119.                         engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  120.                         engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  121.                 }
  122.                 Bluetooth.printf("Distance %d mm\r\n", distance);
  123.         }
  124. }
  125.  
  126. void left_sensor_push()
  127. {
  128.         left_sensor_state = 1;
  129. }
  130. ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  131. void bip()
  132. {
  133.         count.start();
  134.         if (count.read_ms() >= 500)
  135.         {
  136.                 Buzzer = !Buzzer;
  137.                 count.stop();
  138.                 count.reset();
  139.         }
  140. }
  141. class Led
  142. {
  143. public:
  144.         Led(PinName pin)        : _pin(pin)
  145.         {
  146.                 _pin = 0;
  147.         }
  148.         void state(int s)
  149.         {
  150.                 _pin = s;
  151.         }
  152. private:
  153.         DigitalOut _pin;
  154. };
  155. Led Led_front_right(PB_15);
  156. Led Led_front_left(PB_1);
  157. Led Led_back_right(PB_2);
  158. Led Led_back_left(PB_11);
  159.  
  160. int main()
  161. {
  162.         engine.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
  163.         engine.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
  164.         bips = &bip;
  165.         left_sensor.mode(PullUp);
  166.         left_sensor.attach_deasserted( &left_sensor_push );
  167.         left_sensor.setSampleFrequency();
  168.        
  169.        
  170.         right_sensor.mode(PullUp);
  171.         right_sensor.setSampleFrequency();
  172.         sensor.startUpdates();
  173.         for (;;)
  174.         {
  175.                
  176.                 sensor.checkDistance();
  177.                 //bip();
  178.                 collision_sensor();
  179.                 Led_front_right.state(1);
  180.                 Led_front_left.state(1);
  181.                 Led_back_right.state(1);
  182.                 Led_back_left.state(1);
  183.                
  184.         }
  185. }

odpowiedź "robot"

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

captcha