robot

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

odpowiedź "robot"

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

captcha