robot

z foreste, 11 miesiące temu, napisane w C++, wyświetlone 76 razy. [paste_expire] 1 sekunda.
URL https://codetab.e3x.pl/view/5d403a9f 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. Serial Bluetooth(PC_4, PC_5);
  8.  
  9. #define on 1
  10. #define off 0
  11. bool lock = true;
  12. bool lockhead = 1;
  13.  
  14.                          
  15.  
  16. class Gear
  17. {
  18. public:
  19.         void manoeuvre(float step_left, float step_right)
  20.         {
  21.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_A, step_left);        
  22.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_B, step_right);
  23.         }
  24.  
  25.         void cramp(int brake_left, int brake_right)
  26.         {
  27.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_A, brake_left);
  28.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_B, brake_right);
  29.         }
  30.        
  31.         void direction_forward()
  32.         {
  33.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
  34.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
  35.         }
  36.         void direction_backforward()
  37.         {
  38.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_BACKWARD);
  39.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_BACKWARD);
  40.         }
  41. private:
  42.         ArduinoMotorShield _driver;
  43. };
  44.  
  45. class bumper
  46. {
  47. public:
  48.        
  49.         bumper(PinName pin)
  50.                 : _clicker(pin)
  51.         {
  52.                
  53.         }
  54.        
  55.        
  56.         template<typename x>
  57.                 void init(x *object, void(x::*member)(void))
  58.                 {
  59.                
  60.                         _clicker.mode(PullUp);
  61.                         _clicker.attach_deasserted(object, member);
  62.                         _clicker.setSampleFrequency();
  63.                
  64.                 }
  65. private:
  66.         PinDetect  _clicker;
  67.        
  68. };
  69. class Miernik
  70. {
  71.        
  72. public:
  73.         float  check_battery;
  74.         Miernik(PinName pin)
  75.                 : _analog(pin)
  76.         {
  77.         }
  78.         float monitor_battery()
  79.         {
  80.                 check_battery = _analog.read();
  81.                 float resume;
  82.                 return resume = (check_battery * 11.2);
  83.         }
  84.         float monitor()
  85.         {
  86.                 return _analog.read();
  87.         }
  88. private:
  89.         AnalogIn  _analog;
  90.        
  91.        
  92. };
  93. class connector
  94. {
  95. public:
  96.         connector(PinName pin)
  97.                 : _pin(pin)
  98.         {
  99.                 _pin = 0;
  100.         }
  101.         void state(int s)
  102.         {
  103.                 _pin = s;
  104.         }
  105.         void toggle()
  106.         {
  107.                 _pin = !_pin;
  108.                
  109.         }
  110. private:
  111.         DigitalOut _pin;
  112. };
  113.  
  114.  
  115. class push
  116. {
  117. private:
  118.         int tik;
  119. public:
  120.        
  121.         void pushing()
  122.         {
  123.                 tik = 1;
  124.                  
  125.         }
  126.         int detect()
  127.         {
  128.                 return tik;
  129.         }
  130.         void reset()
  131.         {
  132.                 tik = 0;
  133.         }
  134. };
  135.  
  136. connector  * Led_front_right = new connector(PB_15);
  137. connector * Led_front_left = new connector(PB_1);
  138. connector *Led_back_right = new connector(PB_2);
  139. connector *Led_back_left = new connector(PB_11);
  140. connector *Led_red_battery = new connector(PB_4);
  141. connector *Led_yellow_battery = new connector(PB_10);
  142. connector *Led_green_battery = new connector(PA_8);
  143. connector *Buzzer = new connector(PB_12);
  144. Miernik *flashcell = new Miernik(PC_3);
  145. Miernik *Battery = new Miernik(PA_4);
  146. Gear *engine = new Gear;
  147. bumper *left_front_bumper = new bumper(PC_6);
  148. bumper *right_front_bumper = new bumper(PC_8);
  149. bumper *left_back_bumper = new bumper(PA_12);
  150. bumper *right_back_bumper = new bumper(PA_11);
  151. push *front_right = new push;
  152. push *front_left = new push;
  153. push *back_right = new push;
  154. push *back_left = new push;
  155. class master
  156. {
  157. public:
  158.  
  159.         template<typename x>
  160.         void light(x *LED, int state_low, int state_hight, float check)
  161.         {
  162.                 if (flashcell->monitor() <= check)
  163.                 {
  164.                         (*LED)->state(state_hight);
  165.                        
  166.                
  167.                 }
  168.                 else
  169.                 {
  170.                         (*LED)->state(state_low);
  171.                
  172.                
  173.                 }
  174.         }
  175.         template<typename x, typename y>
  176.                 void collision_sensor(x  *object, y *object2, float one_left, float one_right, float two_left, float two_right)
  177.                  
  178.         {
  179.                
  180.                         if ((*object)->detect() == 1)
  181.                 {
  182.                         lock = false;
  183.                         coun3.start();
  184.                         (*object2)->cramp(off, off);
  185.                         (*object2)->manoeuvre(one_left , one_right);
  186.                         if (coun3.read() >= 5)
  187.                         {
  188.                                  (*object)->reset();
  189.                                 lock = true;
  190.                                 coun3.stop();
  191.                                 coun3.reset();
  192.                                 (*object2)->cramp(on, on);
  193.                                 (*object2)->manoeuvre(two_left, two_right);
  194.                         }
  195.                 }
  196.        
  197.  
  198.         }
  199.         void bip()
  200.         {
  201.                 count.start();
  202.                 if (count.read_ms() >= 500)
  203.                 {
  204.                         Led_back_left->toggle();
  205.                         Buzzer->toggle();
  206.                         count.stop();
  207.                         count.reset();
  208.                 }
  209.         }
  210.         void demo(int steper, int cycle)
  211.         {
  212.                 Led_front_right->state(off);
  213.                 Led_front_left->state(off);
  214.                 Led_back_right->state(off);
  215.                 Led_back_left->state(off);
  216.                 wait_ms(100);
  217.                 while (cycle >=0)
  218.                 {
  219.                
  220.                 Led_front_right->state(on);
  221.                 wait_ms(steper);
  222.                 Led_front_right->state(off);
  223.                 Led_front_left->state(on);
  224.                 wait_ms(steper);
  225.                 Led_front_left->state(off);
  226.                 Led_back_right->state(on);
  227.                 wait_ms(steper);
  228.                 Led_back_right->state(off);
  229.                 Led_back_left->state(on);
  230.                 wait_ms(steper);
  231.                 Led_back_left->state(off);
  232.                         --cycle;
  233.                         }
  234.         }
  235. private:
  236. };
  237.  
  238.  
  239.  
  240.  
  241. void dist(int distance)
  242.        
  243. {  
  244.         Bluetooth.printf("Distance %d mm\r\n", distance);
  245.         if (lock == true)
  246.         {
  247.                
  248.                 if ((distance <= 50) && (distance >= 0))
  249.                 {
  250.                        
  251.                         lockhead = 0;
  252.                 }
  253.                 else if(distance <= 50)
  254.                 {
  255.                         lockhead = 1;
  256.                 }
  257.                 if  (lockhead == 0)
  258.                 {
  259.                        
  260.                         count2.start();
  261.                         engine->manoeuvre(0.0, 0.0);
  262.                         if ((count2.read_ms() >= 2000)&& (distance <= 50))
  263.                         {
  264.                                 engine->manoeuvre(0.6, -0.6);
  265.                         }
  266.                         if (count2.read_ms() >= 4000)
  267.                         {
  268.                                 engine->manoeuvre(0.0, 0.0);
  269.                         }
  270.                        
  271.                         if (count2.read_ms() >= 6000)
  272.                         {
  273.                                 lockhead = 1;
  274.                                 count2.stop();
  275.                                 count2.reset();
  276.                                
  277.                         }
  278.                        
  279.                 }
  280.                 if (lockhead == 1)
  281.                 {
  282.                         if (distance <= 50)
  283.                         {
  284.                                 engine->manoeuvre(0.2, 0.2);
  285.                         }
  286.                         else if (distance <= 60)
  287.                         {
  288.                                 engine->manoeuvre(0.3, 0.3);
  289.                         }
  290.                         else if (distance <= 70)
  291.                         {
  292.                                 engine->manoeuvre(0.4, 0.4);
  293.                         }
  294.                         else if (distance <= 80)
  295.                         {
  296.                                 engine->manoeuvre(0.5, 0.5);
  297.                         }
  298.                         else if (distance >= 81)
  299.                         {
  300.                                 engine->manoeuvre(0.6, 0.6);
  301.                         }
  302.        
  303.                
  304.                         Bluetooth.printf("Distance %d mm\r\n", distance);
  305.  
  306.                 }
  307.         }
  308. }
  309.         ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  310.  
  311.  
  312.  
  313.  
  314. int main()
  315. {
  316.         sensor.startUpdates();
  317.         right_front_bumper->init(front_right, &push::pushing);
  318.         left_front_bumper->init(front_left, &push::pushing);
  319.         right_back_bumper->init(front_right, &push::pushing);
  320.         left_back_bumper->init(front_left, &push::pushing);
  321.         master *boss = new master;
  322.        
  323.        
  324.        
  325.        
  326.         //right_bumper.init(&right_sensor_push);
  327.         //main boss;
  328.         boss->demo(300, 2);
  329.        
  330.         for (;;)
  331.         {
  332.                 boss->collision_sensor(&front_left, &engine, 0.6, -0.3, 0.0, 0.0);
  333.                 boss->collision_sensor(&front_right, &engine, 0.6, -0.3, 0.0, 0.0);
  334.                 boss->collision_sensor(&back_left, &engine, 0.6, -0.3, 0.0, 0.0);
  335.                 boss->collision_sensor(&back_right, &engine, 0.6, -0.3, 0.0, 0.0);
  336.                 boss->light(&Led_front_left,off,on,0.3f);
  337.                 boss->light(&Led_front_right, off, on, 0.3f);
  338.                 boss->light(&Led_back_left, off, on, 0.3f);
  339.                 boss->light(&Led_back_right, off, on, 0.3f);
  340.                 sensor.checkDistance();
  341.                
  342.                
  343.                 //Bluetooth.printf("Poziom naładowania baterii %2d V .\n", front_right->detect());
  344.         //      wait(1);
  345.                
  346.         }
  347. }

odpowiedź "robot"

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

captcha