robot

z foreste, 3 miesiące temu, napisane w C++, wyświetlone 13 razy.
URL https://codetab.e3x.pl/view/5378e41a Udostępnij
Pobierz wklejkę lub Pokaż surowy tekst
  1. #include <mbed.h>
  2. #include "ArduinoMotorShield.hpp"
  3. #include "ultrasonic.h"
  4. #include "PinDetectMod.h"
  5.  
  6. Timer count, count2, coun3, count4;
  7. Serial Bluetooth(PC_4, PC_5);
  8.  
  9. #define on 1
  10. #define off 0
  11. bool block_point_one = true;
  12. bool block_point_two = true;
  13. bool block_point_three = true;
  14. bool block_point_four = true;
  15.  
  16.                          
  17.  
  18. class Gear
  19. {
  20. public:
  21.         void manoeuvre(float step_left, float step_right)
  22.         {
  23.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_A, step_left);        
  24.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_B, step_right);
  25.         }
  26.  
  27.         void cramp(int brake_left, int brake_right)
  28.         {
  29.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_A, brake_left);
  30.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_B, brake_right);
  31.         }
  32.        
  33.         void direction_forward()
  34.         {
  35.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
  36.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
  37.         }
  38.         void direction_backforward()
  39.         {
  40.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_BACKWARD);
  41.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_BACKWARD);
  42.         }
  43. private:
  44.         ArduinoMotorShield _driver;
  45. };
  46.  
  47. class bumper
  48. {
  49. public:
  50.        
  51.         bumper(PinName pin)
  52.                 : _clicker(pin)
  53.         {
  54.                
  55.         }
  56.        
  57.        
  58.         template<typename x>
  59.                 void init(x *object, void(x::*member)(void))
  60.                 {
  61.                
  62.                         _clicker.mode(PullUp);
  63.                         _clicker.attach_asserted(object, member);
  64.                         _clicker.setSampleFrequency();
  65.                
  66.                 }
  67. private:
  68.         PinDetect  _clicker;
  69.        
  70. };
  71. class gauge
  72. {
  73.        
  74. public:
  75.         float  check_battery;
  76.         gauge(PinName pin)
  77.                 : _analog(pin)
  78.         {
  79.         }
  80.         float monitor_battery()
  81.         {
  82.                 check_battery = _analog.read();
  83.                 float resume;
  84.                 return resume = (check_battery * 11.2);
  85.         }
  86.         float monitor()
  87.         {
  88.                 return _analog.read();
  89.         }
  90. private:
  91.         AnalogIn  _analog;
  92.        
  93.        
  94. };
  95. class connector
  96. {
  97. public:
  98.         connector(PinName pin)
  99.                 : _pin(pin)
  100.         {
  101.                 _pin = 0;
  102.         }
  103.         void state(int s)
  104.         {
  105.                 _pin = s;
  106.         }
  107.         void toggle()
  108.         {
  109.                 _pin = !_pin;
  110.                
  111.         }
  112. private:
  113.         DigitalOut _pin;
  114. };
  115.  
  116.  
  117. class push
  118. {
  119. private:
  120.         int tik;
  121. public:
  122.        
  123.         void pushing()
  124.         {
  125.                 tik = 1;
  126.                  
  127.         }
  128.         int detect()
  129.         {
  130.                 return tik;
  131.         }
  132.         void reset()
  133.         {
  134.                 tik = 0;
  135.         }
  136. };
  137.  
  138.  
  139. connector  * Led_front_right = new connector(PB_15);
  140. connector * Led_front_left = new connector(PB_1);
  141. connector *Led_back_right = new connector(PB_2);
  142. connector *Led_back_left = new connector(PB_11);
  143. connector *Led_red_battery = new connector(PB_4);
  144. connector *Led_yellow_battery = new connector(PB_10);
  145. connector *Led_green_battery = new connector(PA_8);
  146. connector *Buzzer = new connector(PB_12);
  147. gauge *flashcell = new gauge(PC_3);
  148. gauge *Battery = new gauge(PA_4);
  149. Gear *engine = new Gear;
  150. bumper *left_front_bumper = new bumper(PC_6);
  151. bumper *right_front_bumper = new bumper(PC_8);
  152. bumper *left_back_bumper = new bumper(PA_12);
  153. bumper *right_back_bumper = new bumper(PA_11);
  154. push *front_right = new push;
  155. push *front_left = new push;
  156. push *back_right = new push;
  157. push *back_left = new push;
  158. class Beep
  159. {      
  160. public:
  161.         template<typename t>
  162.                 void bip(t *_connect, int clock)
  163.                 {
  164.                         count.start();
  165.                         if (count.read_ms() >= clock)
  166.                         {
  167.                                 (*_connect)->toggle();
  168.                                 count.stop();
  169.                                 count.reset();
  170.                         }
  171.                 }
  172.         template<typename t>
  173.                 void stop(t *_connect)
  174.                 {
  175.                         (*_connect)->state(0);
  176.                 }
  177. };
  178. Beep *siren = new Beep;
  179. class master
  180. {
  181. private:
  182.         volatile        bool lock_pointer_one = true;
  183.         volatile        bool lock_pointer_two = true;
  184.         volatile bool lock_pointer_three = true;
  185. public:
  186.  
  187.         template<typename x>
  188.                 void light(x *LED, int state_low, int state_hight, float check)
  189.                 {
  190.                         if (flashcell->monitor() <= check)
  191.                         {
  192.                                 (*LED)->state(state_hight);
  193.                        
  194.                
  195.                         }
  196.                         else
  197.                         {
  198.                                 (*LED)->state(state_low);
  199.                
  200.                         }
  201.                 }
  202.         template<typename x, typename y>
  203.                 void collision_sensor(x  *object, y *object2, float one_left, float one_right, float two_left, float two_right, float three_left, float three_right)
  204.                  
  205.                 {
  206.                
  207.                         if ((*object)->detect() != 1)
  208.                         {
  209.                                 coun3.start();
  210.                                 block_point_one = false;
  211.                                 if (coun3.read_ms() >= 2000)
  212.                                 {
  213.                                         (*object2)->manoeuvre(two_left, two_right);
  214.                                 }
  215.                                 if (coun3.read_ms() >= 4000)
  216.                                 {
  217.                                         (*object2)->manoeuvre(one_left, one_right);
  218.                                 }
  219.                        
  220.                        
  221.                                 if (coun3.read_ms() >= 6000)
  222.                                 {
  223.                                         (*object2)->manoeuvre(two_left, two_right);
  224.                                 }
  225.                                 if (coun3.read_ms() >= 8000)
  226.                                 {
  227.                                         (*object2)->manoeuvre(three_left, three_right);
  228.                                 }
  229.                                 if (coun3.read_ms() >= 10000)
  230.                                 {
  231.                                         (*object2)->manoeuvre(two_left, two_right);
  232.                                 }
  233.                                 if (coun3.read_ms() >= 11000)
  234.                                 {
  235.                                         coun3.stop();
  236.                                         coun3.reset();
  237.                                         //(*object)->reset();
  238.                                 //      block_point_one = true;
  239.                                 }
  240.                                
  241.                         }
  242.                        
  243.  
  244.                 }
  245.        
  246.         void demo(int steper, int cycle)
  247.         {
  248.                 Led_front_right->state(off);
  249.                 Led_front_left->state(off);
  250.                 Led_back_right->state(off);
  251.                 Led_back_left->state(off);
  252.                 wait_ms(100);
  253.                 while (cycle >= 0)
  254.                 {
  255.                
  256.                         Led_front_right->state(on);
  257.                         wait_ms(steper);
  258.                         Led_front_right->state(off);
  259.                         Led_front_left->state(on);
  260.                         wait_ms(steper);
  261.                         Led_front_left->state(off);
  262.                         Led_back_right->state(on);
  263.                         wait_ms(steper);
  264.                         Led_back_right->state(off);
  265.                         Led_back_left->state(on);
  266.                         wait_ms(steper);
  267.                         Led_back_left->state(off);
  268.                         --cycle;
  269.                 }
  270.         }
  271.         template<typename an, typename LedR, typename LedG, typename LedY>
  272.                 void battery_indicator(an *object, LedR *Red, LedG *Green, LedY *Yellow)
  273.                 {
  274.                         if ((*object)->monitor_battery() >= 8.0)
  275.                         {
  276.                                 (*Green)->state(1);
  277.                                 (*Yellow)->state(0);
  278.                                 (*Red)->state(0);
  279.                                 siren->stop(&Buzzer);
  280.                         }
  281.                         else if (((*object)->monitor_battery() <= 8.0) &&((*object)->monitor_battery() >= 7.6))
  282.                         {
  283.                                 (*Green)->state(0);
  284.                                 (*Yellow)->state(1);
  285.                                 (*Red)->state(0);
  286.                                 siren->stop(&Buzzer);
  287.                         }
  288.                         else if (((*object)->monitor_battery() <= 7.6) &&((*object)->monitor_battery() >= 7.2))
  289.                         {
  290.                                 siren->bip(&Buzzer, 500);
  291.                                 (*Yellow)->state(0);
  292.                                 (*Red)->state(1);
  293.                                 Bluetooth.printf("I have a discharged battery, charge me !! \r\n");
  294.                         }
  295.                 }
  296. };
  297.  
  298.  
  299.  
  300.  
  301. void dist(int distance)
  302.        
  303. {  
  304.        
  305.         if (block_point_one == true)
  306.         {
  307.                
  308.                 if ((distance <= 20) && (block_point_three == true))
  309.                 {
  310.                        
  311.                         block_point_two = false;
  312.                 }
  313.                                 if(block_point_two == false)
  314.                 {
  315.                         count2.start();
  316.                                  
  317.                         if (count2.read_ms() >= 2000)
  318.                         {
  319.                                 engine->manoeuvre(0.0, 0.0);
  320.                                 //block_point_three = false;
  321.                                        
  322.                         }
  323.                         if (count2.read_ms() >= 3000)
  324.                         {
  325.                                 //block_point_two = 1;
  326.                                 //block_point_one = 1;
  327.                                 engine->manoeuvre(0.3, -0.3);
  328.                                
  329.                         }
  330.                         if (count2.read_ms() >= 4000)
  331.                         {
  332.                                 engine->manoeuvre(0.0, 0.0);
  333.                                 //block_point_three = false;
  334.                                        
  335.                         }
  336.                         if (count2.read_ms() >= 5000)
  337.                         {
  338.                                 //block_point_two = 1;
  339.                                 //block_point_one = 1;
  340.                                 engine->manoeuvre(-0.3, -0.3);
  341.                                 ;
  342.                         }
  343.                         if (count2.read_ms() >= 6000)
  344.                         {
  345.                                 //block_point_two = 1;
  346.                                 //block_point_one = 1;
  347.                                 //engine->manoeuvre(0.3, -0.3);
  348.                                 count2.stop();
  349.                                 count2.reset();
  350.                         }
  351. //                      if (block_point_three == false)
  352. //                      {
  353. //                              if (distance <= 50)
  354. //                              {
  355. //                                      engine->manoeuvre(-0.3, -0.3);
  356. //                              }
  357. //                              if ((distance >= 50) && (distance <= 70))
  358. //                              {
  359. //                                      engine->manoeuvre(0.3, -0.3);
  360. //                                      
  361. //                                      
  362. //                              }
  363. //                      
  364. //                      }
  365.                        
  366.                         //                              if (count2.read_ms() >= 2000)
  367.                         //                              {
  368.                         //                                      lockhead2 = 0;
  369.                         //                                      engine->manoeuvre(0.6, -0.6);
  370.                         //                              }
  371.                         //                              if (count2.read_ms() >= 4000)
  372.                         //                              {
  373.                         //                                      engine->manoeuvre(0.0, 0.0);
  374.                         //                              }
  375.                          
  376.                         //                              if (count2.read_ms() >= 5000)
  377.                         //                              {
  378.                         //                                      block_point_two = 1;
  379.                         //                                      block_point_one = 1;
  380.                         //                                      count2.stop();
  381.                         //                                      count2.reset();
  382.                         //                              }
  383.                        
  384.                        
  385.                 }
  386.                 if (block_point_two == true)
  387.                 {
  388.                         if (distance < 30)
  389.                         {
  390.                                 engine->manoeuvre(0.6, 0.6);
  391.                         }
  392.                         else if (distance <= 40)
  393.                         {
  394.                                 engine->manoeuvre(0.7, 0.7);
  395.                         }
  396.                         else if (distance <= 60)
  397.                         {
  398.                                 engine->manoeuvre(0.8, 0.8);
  399.                         }
  400.                         else if (distance <= 80)
  401.                         {
  402.                                 engine->manoeuvre(0.9, 0.9);
  403.                         }
  404.                         else  //if(distance >= 81)
  405.                                 {
  406.                                         engine->manoeuvre(1.0, 1.0);
  407.                                 }
  408.        
  409.                
  410.                         //Bluetooth.printf("Distance %d mm\r\n", distance);
  411.  
  412.                 }
  413.         }
  414.  
  415. }
  416. ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  417.  
  418.  
  419.  
  420.  
  421. int main()
  422. {
  423.         //sensor.startUpdates();
  424.         right_front_bumper->init(front_right, &push::pushing);
  425.         left_front_bumper->init(front_left, &push::pushing);
  426.         right_back_bumper->init(front_right, &push::pushing);
  427.         left_back_bumper->init(front_left, &push::pushing);
  428.         master *boss = new master;
  429.        
  430.        
  431.        
  432.        
  433.         //right_bumper.init(&right_sensor_push);
  434.         //main boss;
  435.         boss->demo(300, 2);
  436.        
  437.         for (;;)
  438.         {
  439.        
  440.                
  441.        
  442.                 boss->collision_sensor(&front_left, &engine, -0.4, -0.4, 0.0, 0.0, -0.5, 0.5);
  443.                 boss->collision_sensor(&front_right, &engine, -0.4, -0.4, 0.0, 0.0, 0.5, -0.5);
  444.                 boss->collision_sensor(&back_left, &engine, -0.4, -0.4, 0.0, 0.0, 0.5, -0.5);
  445.                 boss->collision_sensor(&back_right, &engine, -0.4, -0.4, 0.0, 0.0, -0.5, 0.5);
  446.                 boss->light(&Led_front_left, off, on, 0.4f);
  447.                 boss->light(&Led_front_right, off, on, 0.4f);
  448.                 boss->light(&Led_back_left, off, on, 0.4f);
  449.                 boss->light(&Led_back_right, off, on, 0.4f);
  450.                 boss->battery_indicator(&Battery, &Led_red_battery, &Led_green_battery, &Led_yellow_battery);
  451.                 //sensor.checkDistance();
  452.                 //Led_red_battery->state(0);
  453.                  
  454.                
  455.                
  456.                 //Bluetooth.printf("Poziom naładowania baterii %2d V .\n", Battery->monitor_battery());
  457.         //      wait(1);
  458.                
  459.         }
  460. }

odpowiedź "robot"

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

captcha