robot

z foreste, 9 miesiące temu, napisane w C++, wyświetlone 117 razy.
URL https://codetab.e3x.pl/view/5ce359b7 Udostępnij
Pobierz wklejkę lub Pokaż surowy tekst
  1. // TODO: Napisać od nowa obługe wykrywanie przezkód str 313
  2. #include <mbed.h>
  3. #include "ArduinoMotorShield.hpp"
  4. #include "ultrasonic.h"
  5. #include "PinDetect.h"
  6.  
  7. Timer count, count2, coun3, count4, debugs;
  8. Serial Bluetooth(PC_4, PC_5);
  9.  
  10. #define on 1
  11. #define off 0
  12. bool block_point_one = true;
  13. bool block_point_two = true;
  14. bool block_point_three = true;
  15. bool block_point_four = true;
  16. bool block_point_five = true;
  17. bool block_point_six = true;
  18. int length;
  19.  
  20.                          
  21.  
  22. class Gear
  23. {
  24. public:
  25.         void manoeuvre(float step_left, float step_right)
  26.         {
  27.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_A, step_left);        
  28.                 _driver.SetMotorPower(ArduinoMotorShield::MOTOR_B, step_right);
  29.         }
  30.  
  31.         void cramp(int brake_left, int brake_right)
  32.         {
  33.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_A, brake_left);
  34.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_B, brake_right);
  35.         }
  36.        
  37.         void ammeter(int brake_left, int brake_right)
  38.         {
  39.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_A, brake_left);
  40.                 _driver.SetBrake(ArduinoMotorShield::MOTOR_B, brake_right);
  41.         }
  42.         void direction_forward()
  43.         {
  44.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_FORWARD);
  45.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_FORWARD);
  46.         }
  47.         void direction_backforward()
  48.         {
  49.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_A, ArduinoMotorShield::MOTOR_BACKWARD);
  50.                 _driver.SetMotorPolarity(ArduinoMotorShield::MOTOR_B, ArduinoMotorShield::MOTOR_BACKWARD);
  51.         }
  52. private:
  53.         ArduinoMotorShield _driver;
  54. };
  55.  
  56. class bumper
  57. {
  58. public:
  59.        
  60.         bumper(PinName pin, PinMode mode, int state)
  61.                 : _clicker(pin)
  62.         {
  63.                
  64.         }
  65.         template<typename x>
  66.                 void init(x *object, void(x::*member)(void))
  67.                 {
  68.                         _clicker.mode(PullUp);
  69.                         _clicker.attach_asserted(object, member);
  70.                         _clicker.setSampleFrequency();
  71.                 }
  72. private:
  73.         PinDetect  _clicker;
  74. };
  75. class gauge
  76. {
  77.         public:
  78.         float  check_battery;
  79.         gauge(PinName pin)
  80.                 : _analog(pin)
  81.         {
  82.         }
  83.         float monitor_battery()
  84.         {
  85.                 check_battery = _analog.read();
  86.                 float resume;
  87.                 return resume = (check_battery * 11.2);
  88.         }
  89.         float   monitor()
  90.         {
  91.                 return _analog.read();
  92.         }
  93. private:
  94.         AnalogIn  _analog;
  95.        
  96.        
  97. };
  98. class connector
  99. {
  100. public:
  101.         connector(PinName pin)
  102.                 : _pin(pin)
  103.         {
  104.                 _pin = 0;
  105.         }
  106.         void state(int s)
  107.         {
  108.                 _pin = s;
  109.         }
  110.         void toggle()
  111.         {
  112.                 _pin = !_pin;
  113.                
  114.         }
  115. private:
  116.         DigitalOut _pin;
  117. };
  118.  
  119.  
  120. class push
  121. {
  122. private:
  123.         int tik;
  124. public:
  125.        
  126.         void pushing()
  127.         {
  128.                 tik = 1;
  129.                  
  130.         }
  131.         int detect()
  132.         {
  133.                 return tik;
  134.         }
  135.         void reset()
  136.         {
  137.                 tik = 0;
  138.         }
  139. };
  140.  
  141.  
  142. connector  * Led_front_right = new connector(PB_15);
  143. connector * Led_front_left = new connector(PB_1);
  144. connector *Led_back_right = new connector(PB_2);
  145. connector *Led_back_left = new connector(PB_11);
  146. connector *Led_red_battery = new connector(PB_4);
  147. connector *Led_yellow_battery = new connector(PB_10);
  148. connector *Led_green_battery = new connector(PA_8);
  149. connector *Buzzer = new connector(PB_12);
  150. gauge *flashcell = new gauge(PC_3);
  151. gauge *Battery = new gauge(PA_4);
  152. Gear *engine = new Gear;
  153. bumper *right_front_bumper = new bumper(PC_6, PullUp, 0);
  154. bumper *left_front_bumper = new bumper(PC_8, PullUp, 0);
  155. bumper *left_back_bumper = new bumper(PA_12, PullUp, 0);
  156. bumper *right_back_bumper = new bumper(PA_11, PullUp, 0);
  157. push *front_right = new push;
  158. push *front_left = new push;
  159. push *back_right = new push;
  160. push *back_left = new push;
  161. class Beep
  162. {      
  163. public:
  164.         template<typename t>
  165.                 void bip(t *_connect, int clock)
  166.                 {
  167.                         count.start();
  168.                         if (count.read_ms() >= clock)
  169.                         {
  170.                                 (*_connect)->toggle();
  171.                                 count.stop();
  172.                                 count.reset();
  173.                         }
  174.                 }
  175.         template<typename t>
  176.                 void stop(t *_connect)
  177.                 {
  178.                         (*_connect)->state(0);
  179.                 }
  180. };
  181. Beep *siren = new Beep;
  182. class master
  183. {
  184. private:
  185.  
  186. public:
  187.  
  188.         template<typename x>
  189.                 void light(x *LED, int state_low, int state_hight, float check)
  190.                 {
  191.                         if (flashcell->monitor() <= check)
  192.                         {
  193.                                 (*LED)->state(state_hight);
  194.                         }
  195.                         else
  196.                         {
  197.                                 (*LED)->state(state_low);
  198.                         }
  199.                 }
  200.        
  201.         template<typename x >
  202.                 void layover(x *Led, int state_low, int state_hight, int cutout)
  203.         {
  204.                 if (cutout == 0)
  205.                         (*Led)->state(state_low);
  206.                 if (cutout == 1)
  207.                 {
  208.                         (*Led)->state(state_hight);
  209.                 }
  210.         }
  211.         template<typename x, typename y, typename t>
  212.                 void collision_sensor(x  *object, y *object2,t *object3, float one_left, float one_right, float two_left, float two_right, float three_left, float three_right)
  213.                 {
  214.                         if ((*object)->detect() == 1)
  215.                         {
  216.                                 coun3.start();
  217.                                 block_point_one = false;
  218.                                 if ((coun3.read_ms() >= 1) && (coun3.read_ms() <= 1000))
  219.                                 {
  220.                                         (*object2)->manoeuvre(two_left, two_right);
  221.                                 }
  222.                                 if ((coun3.read_ms() >= 1000) && (coun3.read_ms() <= 3000))
  223.                                 {
  224.                                         (*object2)->manoeuvre(one_left, one_right);
  225.                                 }
  226.                                 if ((coun3.read_ms() >= 3000) && (coun3.read_ms() <= 5000))
  227.                                 {
  228.                                         (*object2)->manoeuvre(two_left, two_right);
  229.                                 }
  230.                                 if ((coun3.read_ms() >= 5000) && (coun3.read_ms() <= 5500))
  231.                                 {
  232.                                         (*object2)->manoeuvre(three_left, three_right);
  233.                                 }
  234.                                 if ((coun3.read_ms() >= 5500) && (coun3.read_ms() <= 8000))
  235.                                 {
  236.                                         (*object2)->manoeuvre(two_left, two_right);
  237.                                 }
  238.                                 if (coun3.read_ms() >= 8000)
  239.                                 {
  240.                                         (*object)->reset();
  241.                                         coun3.stop();
  242.                                         coun3.reset();
  243.                                         block_point_one = true;
  244.                                 }
  245.                         }
  246.                 }
  247.         void demo(int steper, int cycle)
  248.         {
  249.                 Led_front_right->state(off);
  250.                 Led_front_left->state(off);
  251.                 Led_back_right->state(off);
  252.                 Led_back_left->state(off);
  253.                 wait_ms(100);
  254.                 while (cycle >= 0)
  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, typename s>
  272.                 void battery_indicator(an *object, LedR *Red, LedG *Green, LedY *Yellow, s *signal)
  273.                 {
  274.                         if ((*object)->monitor_battery() >= 8.3)
  275.                         {
  276.                                 (*Green)->state(1);
  277.                                 (*Yellow)->state(0);
  278.                                 (*Red)->state(0);
  279.                                 (*signal)->stop(&Buzzer);
  280.                         }
  281.                         else if (((*object)->monitor_battery() <= 7.87) &&((*object)->monitor_battery() >= 7.44))
  282.                         {
  283.                                 (*Green)->state(0);
  284.                                 (*Yellow)->state(1);
  285.                                 (*Red)->state(0);
  286.                                 (*signal)->stop(&Buzzer);
  287.                         }
  288.                         else if (((*object)->monitor_battery() <= 7.44) &&((*object)->monitor_battery() >= 7.01))
  289.                         {
  290.                                 (*signal)->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. master *boss = new master;
  298. void road()
  299. {
  300.         if (block_point_one == true)
  301.         {
  302.                 if ((length < 100) && (block_point_three == true))
  303.                 {
  304.                         engine->manoeuvre(0.0, 0.0);
  305.                         engine->cramp(1, 1);   
  306.                         block_point_two = false;
  307.                         block_point_three = false;
  308.                         block_point_five = true;
  309.                 }else
  310.                 {
  311.                         engine->cramp(0, 0);
  312.                 }
  313.                 if (block_point_two == false)
  314.                 {
  315.                         boss->layover(&Led_front_right, off, on, 1);
  316.                         boss->layover(&Led_front_left, off, on, 1);
  317.                         boss->layover(&Led_back_right, off, on, 1);
  318.                         boss->layover(&Led_back_left, off, on, 1);
  319.                         count2.start();
  320.                         if ((length <= 140) && (count2.read_ms() >= 3000)&&(block_point_five == true))
  321.                         {
  322.                                 engine->cramp(0, 0);   
  323.                                 engine->manoeuvre(-0.3, -0.3);
  324.                                 block_point_four = true;
  325.                         }
  326.                         if ((length >= 140) && ( block_point_four == true))
  327.                         {
  328.                                 count2.reset();
  329.                                 engine->manoeuvre(0.0, 0.0);
  330.                                 block_point_four = false;
  331.                                 block_point_six = true;
  332.                         }
  333.                
  334.                         if ((length >= 140)&& (length <= 200) && (count2.read_ms() >= 3000) && (block_point_six == true))
  335.                         {
  336.                                 block_point_five = false;
  337.                                 engine->manoeuvre(0.3, -0.3);
  338.                         }
  339.                         if (length >= 200)
  340.                         {
  341.                                 block_point_six = false;
  342.                                 block_point_three = true;
  343.                                 block_point_two = true;
  344.                                 count2.stop();
  345.                                 count2.reset();
  346.                                 boss->layover(&Led_front_right, off, on, 0);
  347.                                 boss->layover(&Led_front_left, off, on, 0);
  348.                                 boss->layover(&Led_back_right, off, on, 0);
  349.                                 boss->layover(&Led_back_left, off, on, 0);
  350.                         }
  351.                 }
  352.                                 if(block_point_two == true)
  353.                         {
  354.                                 if (length <= 100)
  355.                                 {
  356.                                         engine->manoeuvre(0.3, 0.3);
  357.                                 }
  358.                                 else if (length <= 150)
  359.                                 {
  360.                                         engine->manoeuvre(0.5, 0.5);
  361.                                 }
  362.                                 else // if(length >= 81)
  363.                                         {
  364.                                                 engine->manoeuvre(0.7, 0.7);
  365.                                         }
  366.                         }
  367.         }
  368. }
  369. void dist(int distance)
  370. {  
  371.         length = distance;
  372. }
  373. ultrasonic sensor(PB_13, PB_14, 0.1, 0.25, &dist);
  374. int main()
  375. {
  376.         sensor.startUpdates();
  377.         right_front_bumper->init(front_right, &push::pushing);
  378.         left_front_bumper->init(front_left, &push::pushing);
  379.         right_back_bumper->init(back_right, &push::pushing);
  380.         left_back_bumper->init(back_left, &push::pushing);
  381.         boss->demo(300, 2);
  382.        
  383.         for (;;)
  384.         {              
  385.                 sensor.checkDistance();
  386.                 road();
  387.                 boss->collision_sensor(&front_left, &engine, &left_front_bumper , -0.5, -0.5, 0.0, 0.0, 0.5, -0.5);
  388.                 boss->collision_sensor(&front_right, &engine, &right_front_bumper, -0.5, -0.5, 0.0, 0.0, -0.5, 0.5);
  389.                 boss->collision_sensor(&back_left, &engine, &left_back_bumper, 0.5, 0.5, 0.0, 0.0, 0.5, -0.5);
  390.                 boss->collision_sensor(&back_right, &engine, &right_back_bumper, 0.5, 0.5, 0.0, 0.0, -0.5, 0.5);
  391.                 boss->light(&Led_front_left, off, on, 0.4f);
  392.                 boss->light(&Led_front_right, off, on, 0.4f);
  393.                 boss->light(&Led_back_left, off, on, 0.4f);
  394.                 boss->light(&Led_back_right, off, on, 0.4f);
  395.                 boss->battery_indicator(&Battery, &Led_red_battery, &Led_green_battery, &Led_yellow_battery, &siren);
  396.         }
  397. }

odpowiedź "robot"

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

captcha