robot

z foreste, 11 miesiące temu, napisane w C++, wyświetlone 116 razy. [paste_expire] 1 sekunda.
URL https://codetab.e3x.pl/view/b8544d10 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.                                
  190.                                 coun3.stop();
  191.                                 coun3.reset();
  192.                                 //(*object2)->cramp(on, on);
  193.                                 (*object2)->manoeuvre(two_left, two_right);
  194.                                 lock = true;
  195.                         }
  196.                 }
  197.        
  198.  
  199.         }
  200.         void bip()
  201.         {
  202.                 count.start();
  203.                 if (count.read_ms() >= 500)
  204.                 {
  205.                         Led_back_left->toggle();
  206.                         Buzzer->toggle();
  207.                         count.stop();
  208.                         count.reset();
  209.                 }
  210.         }
  211.         void demo(int steper, int cycle)
  212.         {
  213.                 Led_front_right->state(off);
  214.                 Led_front_left->state(off);
  215.                 Led_back_right->state(off);
  216.                 Led_back_left->state(off);
  217.                 wait_ms(100);
  218.                 while (cycle >=0)
  219.                 {
  220.                
  221.                 Led_front_right->state(on);
  222.                 wait_ms(steper);
  223.                 Led_front_right->state(off);
  224.                 Led_front_left->state(on);
  225.                 wait_ms(steper);
  226.                 Led_front_left->state(off);
  227.                 Led_back_right->state(on);
  228.                 wait_ms(steper);
  229.                 Led_back_right->state(off);
  230.                 Led_back_left->state(on);
  231.                 wait_ms(steper);
  232.                 Led_back_left->state(off);
  233.                         --cycle;
  234.                         }
  235.         }
  236. private:
  237. };
  238.  
  239.  
  240.  
  241.  
  242. void dist(int distance)
  243.        
  244. {  
  245.         Bluetooth.printf("Distance %d mm\r\n", distance);
  246.         if (lock == true)
  247.         {
  248.                
  249.                 if ((distance <= 50) && (distance >= 0))
  250.                 {
  251.                        
  252.                         lockhead = 0;
  253.                 }
  254.                 else if(distance <= 30)
  255.                 {
  256.                         lockhead = 1;
  257.                 }
  258.                 if  (lockhead == 0)
  259.                 {
  260.                        
  261.                         count2.start();
  262.                         engine->manoeuvre(0.0, 0.0);
  263.                         if ((count2.read_ms() >= 2000)&& (distance >= 20))
  264.                         {
  265.                                 engine->manoeuvre(0.6, -0.6);
  266.                         }
  267.                         if (count2.read_ms() >= 4000)
  268.                         {
  269.                                 engine->manoeuvre(0.0, 0.0);
  270.                         }
  271.                        
  272.                         if (count2.read_ms() >= 5000)
  273.                         {
  274.                                 lockhead = 1;
  275.                                 count2.stop();
  276.                                 count2.reset();
  277.                                
  278.                         }
  279.                        
  280.                 }
  281.                 if (lockhead == 1)
  282.                 {
  283.                         if (distance <= 20)
  284.                         {
  285.                                 engine->manoeuvre(0.2, 0.2);
  286.                         }
  287.                         else if (distance <= 40)
  288.                         {
  289.                                 engine->manoeuvre(0.3, 0.3);
  290.                         }
  291.                         else if (distance <= 60)
  292.                         {
  293.                                 engine->manoeuvre(0.4, 0.4);
  294.                         }
  295.                         else if (distance <= 80)
  296.                         {
  297.                                 engine->manoeuvre(0.5, 0.5);
  298.                         }
  299.                         else if (distance >= 81)
  300.                         {
  301.                                 engine->manoeuvre(1.0, 1.0);
  302.                         }
  303.        
  304.                
  305.                         Bluetooth.printf("Distance %d mm\r\n", distance);
  306.  
  307.                 }
  308.         }
  309. }
  310.         ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  311.  
  312.  
  313.  
  314.  
  315. int main()
  316. {
  317.         sensor.startUpdates();
  318.         right_front_bumper->init(front_right, &push::pushing);
  319.         left_front_bumper->init(front_left, &push::pushing);
  320.         right_back_bumper->init(front_right, &push::pushing);
  321.         left_back_bumper->init(front_left, &push::pushing);
  322.         master *boss = new master;
  323.        
  324.        
  325.        
  326.        
  327.         //right_bumper.init(&right_sensor_push);
  328.         //main boss;
  329.         boss->demo(300, 2);
  330.        
  331.         for (;;)
  332.         {
  333.        
  334.                 sensor.checkDistance();
  335.        
  336.                 boss->collision_sensor(&front_left, &engine, 0.6, -0.3, 0.0, 0.0);
  337.                 boss->collision_sensor(&front_right, &engine, 0.6, -0.3, 0.0, 0.0);
  338.                 boss->collision_sensor(&back_left, &engine, 0.6, -0.3, 0.0, 0.0);
  339.                 boss->collision_sensor(&back_right, &engine, 0.6, -0.3, 0.0, 0.0);
  340.                 boss->light(&Led_front_left,off,on,0.4f);
  341.                 boss->light(&Led_front_right, off, on, 0.4f);
  342.                 boss->light(&Led_back_left, off, on, 0.4f);
  343.                 boss->light(&Led_back_right, off, on, 0.4f);
  344.                  
  345.                
  346.                
  347.                 //Bluetooth.printf("Poziom naładowania baterii %2d V .\n", front_right->detect());
  348.         //      wait(1);
  349.                
  350.         }
  351. }

odpowiedź "robot"

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

captcha