robot

z foreste, 6 miesiące temu, napisane w C++, wyświetlone 69 razy. [paste_expire] 1 sekunda.
URL https://codetab.e3x.pl/view/8ee2ca0c 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. Timer count , count2, coun3 ;
  7. AnalogIn sensor_light(PC_3);
  8. Serial Bluetooth(PC_4, PC_5);
  9. static int left_sensor_state = 0;
  10. static int right_sensor_state = 0;
  11. #define on 1
  12. #define off 0
  13. bool lock = true;
  14.  
  15. void right_sensor_push()
  16. {
  17.         right_sensor_state = 1;
  18. }
  19.  
  20. void left_sensor_push()
  21. {
  22.         left_sensor_state = 1;
  23. }
  24. class Gear
  25. {
  26. public:
  27.         void manoeuvre(float step_left, float step_right)
  28.         {
  29.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_A, step_left);        
  30.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_B, step_right);
  31.         }
  32.  
  33.         void cramp(int brake_left, int brake_right)
  34.         {
  35.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_A, brake_left);
  36.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_B, brake_right);
  37.         }
  38.        
  39.         void direction_forward()
  40.         {
  41.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
  42.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
  43.         }
  44.         void direction_backforward()
  45.         {
  46.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_BACKWARD);
  47.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_BACKWARD);
  48.         }
  49. private:
  50.         ArduinoMotorShield _driver;
  51. };
  52. class Miernik
  53. {
  54.        
  55. public:
  56.         Miernik(PinName pin)
  57.                 : _analog(pin)
  58.         {
  59.         }
  60.         float xx()
  61.         {
  62.                 return _analog.read();
  63.         }
  64. private:
  65.         AnalogIn  _analog;
  66.        
  67. };
  68. class connector
  69. {
  70. public:
  71.         connector(PinName pin)
  72.                 : _pin(pin)
  73.         {
  74.                 _pin = 0;
  75.         }
  76.         void state(int s)
  77.         {
  78.                 _pin = s;
  79.         }
  80.         void toggle()
  81.         {
  82.                 _pin = !_pin;
  83.                
  84.         }
  85. private:
  86.         DigitalOut _pin;
  87. };
  88.  
  89. class bumper
  90. {
  91. public:
  92.        
  93.         bumper(PinName pin)
  94.                 : _clicker(pin)
  95.         {
  96.                
  97.         }
  98.        
  99.         void Left()
  100.         {
  101.                
  102.                 _clicker.mode(PullUp);
  103.                 _clicker.attach_deasserted(&left_sensor_push);
  104.                 _clicker.setSampleFrequency();
  105.                
  106.         }
  107.        
  108.         void Right()
  109.         {
  110.                
  111.                 _clicker.mode(PullUp);
  112.                 _clicker.attach_deasserted(&right_sensor_push);
  113.                 _clicker.setSampleFrequency();
  114.                
  115.         }
  116. private:
  117.         PinDetect  _clicker;
  118. };
  119.  
  120. connector Led_front_right(PB_15);
  121. connector Led_front_left(PB_1);
  122. connector Led_back_right(PB_2);
  123. connector Led_back_left(PB_11);
  124. connector Buzzer(PB_12);
  125. Miernik pomiar(PC_3);
  126. Gear engine;
  127. bumper left_bumper(PC_6);
  128. bumper right_bumper(PC_8);
  129.  
  130. void collision_sensor()
  131. {
  132.         if (right_sensor_state == 1)
  133.         {
  134.                 lock = false;
  135.                 coun3.start();
  136.                 engine.cramp(off, off);
  137.                 engine.manoeuvre(-0.6, -0.3);
  138.                 if (coun3.read() >= 5)
  139.                 {
  140.                         right_sensor_state = 0;
  141.                         lock = true;
  142.                         coun3.stop();
  143.                         coun3.reset();
  144.                 }
  145.         }
  146.        
  147.         if (left_sensor_state == 1)
  148.         {
  149.                 lock = false;
  150.                 coun3.start();
  151.                 engine.cramp(off, off);
  152.                 engine.manoeuvre(-0.3, -0.6);
  153.                 if (coun3.read() >= 5)
  154.                 {
  155.                         left_sensor_state = 0;
  156.                         lock = true;
  157.                         coun3.stop();
  158.                         coun3.reset();
  159.                 }
  160.         }
  161. }
  162.  
  163. void dist(int distance)
  164.        
  165. {  
  166.         /*
  167.         if (lock == true)
  168.         {
  169.                
  170.                 if ((distance <= 30) && (distance >= 0))
  171.                 {
  172.                         engine.SetBrake(ArduinoMotorShield::MOTOR_A, 1);
  173.                         engine.SetBrake(ArduinoMotorShield::MOTOR_B, 1);
  174.                         bips();
  175.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.0);
  176.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.0);
  177.                         count2.start();
  178.                         if (count2.read_ms() >= 3000)
  179.                         {
  180.                                 engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  181.                                 engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  182.                                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  183.                                 engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, -0.6);
  184.                                 if (count2.read_ms() >= 5000)
  185.                                 {
  186.                                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, -0.6);
  187.                                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
  188.                                 }
  189.                        
  190.                                 if ((count2.read_ms() >= 7000) && (distance <= 50))
  191.                                 {
  192.                                         count2.stop();
  193.                                         count2.reset();
  194.                                 }
  195.                         }
  196.                 }
  197.                 else    if (distance <= 50)
  198.                 {
  199.                
  200.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.2);
  201.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.2);
  202.                 }
  203.                 else if (distance <= 60)
  204.                 {
  205.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.3);
  206.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.3);
  207.                 }
  208.                 else if (distance <= 70)
  209.                 {
  210.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.4);
  211.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.4);
  212.                 }
  213.                 else if (distance <= 80)
  214.                 {
  215.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.5);
  216.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.5);
  217.                 }
  218.                 else if (distance <= 90)
  219.                 {
  220.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.6);
  221.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.6);
  222.                 }
  223.                 else if (distance <= 100)
  224.                 {
  225.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.7);
  226.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.7);
  227.                 }
  228.        
  229.                 else
  230.                 {
  231.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_A, 0.8);
  232.                         engine.SetMotorPower(ArduinoMotorShield::MOTOR_B, 0.8);
  233.                         engine.SetBrake(ArduinoMotorShield::MOTOR_A, 0);
  234.                         engine.SetBrake(ArduinoMotorShield::MOTOR_B, 0);
  235.                 }
  236.                 */
  237.                 Bluetooth.printf("Distance %d mm\r\n", distance);
  238.  
  239. }
  240.  
  241. ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  242. void bip()
  243. {
  244.         count.start();
  245.         if (count.read_ms() >= 500)
  246.         {
  247.                 Led_back_left.toggle();
  248.                 Buzzer.toggle();
  249.                 count.stop();
  250.                 count.reset();
  251.         }
  252. }
  253. void demo(int steper, int cycle)
  254. {
  255.         while (cycle >=0)
  256.         {
  257.                
  258.         Led_front_right.state(off);
  259.         Led_front_left.state(off);
  260.         Led_back_right.state(off);
  261.         Led_back_left.state(off);
  262.         wait_ms(100);
  263.         Led_front_right.state(on);
  264.         wait_ms(steper);
  265.         Led_front_right.state(off);
  266.         Led_front_left.state(on);
  267.         wait_ms(steper);
  268.         Led_front_left.state(off);
  269.         Led_back_right.state(on);
  270.         wait_ms(steper);
  271.         Led_back_right.state(off);
  272.         Led_back_left.state(on);
  273.         wait_ms(steper);
  274.         Led_back_left.state(off);
  275.                 --cycle;
  276.                 }
  277.  
  278. }
  279.  
  280.  
  281. int main()
  282. {
  283.         left_bumper.Left();
  284.         right_bumper.Right();
  285.         demo(300, 2);
  286.        
  287.         for (;;)
  288.         {
  289.                
  290.                 sensor.checkDistance();
  291.                 engine.cramp(off, off);
  292.                 //bip();
  293.                 collision_sensor();
  294.                 Led_front_right.state(1);
  295.                 Led_front_left.state(1);
  296.                 Led_back_right.state(1);
  297.                 Led_back_left.state(1);
  298.                 //engine.manoeuvre(-0.3, 0.3);
  299.                
  300.                 //engine.cramp(on,on);
  301.                 Bluetooth.printf("This program runs since %1.2fv seconds.\n", pomiar.xx());
  302.                
  303.         }
  304. }

odpowiedź "robot"

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

captcha