robot

z foreste, 10 miesiące temu, napisane w C++, wyświetlone 118 razy.
URL https://codetab.e3x.pl/view/e4339f59 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 gauge
  70. {
  71.        
  72. public:
  73.         float  check_battery;
  74.         gauge(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.  
  137. connector  * Led_front_right = new connector(PB_15);
  138. connector * Led_front_left = new connector(PB_1);
  139. connector *Led_back_right = new connector(PB_2);
  140. connector *Led_back_left = new connector(PB_11);
  141. connector *Led_red_battery = new connector(PB_4);
  142. connector *Led_yellow_battery = new connector(PB_10);
  143. connector *Led_green_battery = new connector(PA_8);
  144. connector *Buzzer = new connector(PB_12);
  145. gauge *flashcell = new gauge(PC_3);
  146. gauge *Battery = new gauge(PA_4);
  147. Gear *engine = new Gear;
  148. bumper *left_front_bumper = new bumper(PC_6);
  149. bumper *right_front_bumper = new bumper(PC_8);
  150. bumper *left_back_bumper = new bumper(PA_12);
  151. bumper *right_back_bumper = new bumper(PA_11);
  152. push *front_right = new push;
  153. push *front_left = new push;
  154. push *back_right = new push;
  155. push *back_left = new push;
  156. class Beep
  157. {
  158. public:
  159.         template<typename t>
  160.         void bip(t *_connect, int clock)
  161.         {
  162.                 count.start();
  163.                 if (count.read_ms() >= clock)
  164.                 {
  165.                         (*_connect)->toggle();
  166.                         count.stop();
  167.                         count.reset();
  168.                 }
  169.         }
  170.         template<typename t>
  171.         void stop(t *_connect)
  172.         {
  173.                 (*_connect)->state(0);
  174.         }
  175. };
  176. Beep *siren = new Beep;
  177. class master
  178. {
  179. public:
  180.  
  181.         template<typename x>
  182.         void light(x *LED, int state_low, int state_hight, float check)
  183.         {
  184.                 if (flashcell->monitor() <= check)
  185.                 {
  186.                         (*LED)->state(state_hight);
  187.                        
  188.                
  189.                 }
  190.                 else
  191.                 {
  192.                         (*LED)->state(state_low);
  193.                
  194.                
  195.                 }
  196.         }
  197.         template<typename x, typename y>
  198.                 void collision_sensor(x  *object, y *object2, float one_left, float one_right, float two_left, float two_right)
  199.                  
  200.         {
  201.                
  202.                         if ((*object)->detect() == 1)
  203.                 {
  204.                         lock = false;
  205.                         coun3.start();
  206.                         (*object2)->cramp(off, off);
  207.                         (*object2)->manoeuvre(one_left , one_right);
  208.                         if (coun3.read() >= 5)
  209.                         {
  210.                                  (*object)->reset();
  211.                                
  212.                                 coun3.stop();
  213.                                 coun3.reset();
  214.                                 //(*object2)->cramp(on, on);
  215.                                 (*object2)->manoeuvre(two_left, two_right);
  216.                                 lock = true;
  217.                         }
  218.                 }
  219.        
  220.  
  221.         }
  222.        
  223.         void demo(int steper, int cycle)
  224.         {
  225.                 Led_front_right->state(off);
  226.                 Led_front_left->state(off);
  227.                 Led_back_right->state(off);
  228.                 Led_back_left->state(off);
  229.                 wait_ms(100);
  230.                 while (cycle >=0)
  231.                 {
  232.                
  233.                 Led_front_right->state(on);
  234.                 wait_ms(steper);
  235.                 Led_front_right->state(off);
  236.                 Led_front_left->state(on);
  237.                 wait_ms(steper);
  238.                 Led_front_left->state(off);
  239.                 Led_back_right->state(on);
  240.                 wait_ms(steper);
  241.                 Led_back_right->state(off);
  242.                 Led_back_left->state(on);
  243.                 wait_ms(steper);
  244.                 Led_back_left->state(off);
  245.                         --cycle;
  246.                         }
  247.         }
  248.         template<typename an, typename LedR, typename LedG, typename LedY>
  249.         void battery_indicator(an *object, LedR *Red ,LedG *Green, LedY *Yellow)
  250.         {
  251.                 if ((*object)->monitor_battery() >= 8.0)
  252.                 {
  253.                         (*Green)->state(1);
  254.                         (*Yellow)->state(0);
  255.                         (*Red)->state(0);
  256.                         siren->stop(&Buzzer);
  257.                 }else if (((*object)->monitor_battery() <= 8.0) &&((*object)->monitor_battery()>= 7.6))
  258.                 {
  259.                         (*Green)->state(0);
  260.                         (*Yellow)->state(1);
  261.                         (*Red)->state(0);
  262.                         siren->stop(&Buzzer);
  263.                 }else if (((*object)->monitor_battery()<= 7.6) &&((*object)->monitor_battery()>= 7.2))
  264.                 {
  265.                         siren->bip(&Buzzer, 500);
  266.                         (*Yellow)->state(0);
  267.                         (*Red)->state(1);
  268.                         Bluetooth.printf("I have a discharged battery, charge me !! \r\n");
  269.                 }
  270.         }
  271. private:
  272. };
  273.  
  274.  
  275.  
  276.  
  277. void dist(int distance)
  278.        
  279. {  
  280.        
  281.         if (lock == true)
  282.         {
  283.                
  284.                 if ((distance <= 50) && (distance >= 0))
  285.                 {
  286.                        
  287.                         lockhead = 0;
  288.                 }
  289.                 else if(distance <= 30)
  290.                 {
  291.                         lockhead = 1;
  292.                 }
  293.                 if  (lockhead == 0)
  294.                 {
  295.                        
  296.                         count2.start();
  297.                         engine->manoeuvre(0.0, 0.0);
  298.                         if ((count2.read_ms() >= 2000)&& (distance >= 20))
  299.                         {
  300.                                 engine->manoeuvre(0.6, -0.6);
  301.                         }
  302.                         if (count2.read_ms() >= 4000)
  303.                         {
  304.                                 engine->manoeuvre(0.0, 0.0);
  305.                         }
  306.                        
  307.                         if (count2.read_ms() >= 5000)
  308.                         {
  309.                                 lockhead = 1;
  310.                                 count2.stop();
  311.                                 count2.reset();
  312.                                
  313.                         }
  314.                        
  315.                 }
  316.                 if (lockhead == 1)
  317.                 {
  318.                         if (distance <= 20)
  319.                         {
  320.                                 engine->manoeuvre(0.2, 0.2);
  321.                         }
  322.                         else if (distance <= 40)
  323.                         {
  324.                                 engine->manoeuvre(0.3, 0.3);
  325.                         }
  326.                         else if (distance <= 60)
  327.                         {
  328.                                 engine->manoeuvre(0.4, 0.4);
  329.                         }
  330.                         else if (distance <= 80)
  331.                         {
  332.                                 engine->manoeuvre(0.5, 0.5);
  333.                         }
  334.                         else if (distance >= 81)
  335.                         {
  336.                                 engine->manoeuvre(1.0, 1.0);
  337.                         }
  338.        
  339.                
  340.                         //Bluetooth.printf("Distance %d mm\r\n", distance);
  341.  
  342.                 }
  343.         }
  344. }
  345.         ultrasonic sensor(PB_13, PB_14, .1, 1, &dist);
  346.  
  347.  
  348.  
  349.  
  350. int main()
  351. {
  352.         sensor.startUpdates();
  353.         right_front_bumper->init(front_right, &push::pushing);
  354.         left_front_bumper->init(front_left, &push::pushing);
  355.         right_back_bumper->init(front_right, &push::pushing);
  356.         left_back_bumper->init(front_left, &push::pushing);
  357.         master *boss = new master;
  358.        
  359.        
  360.        
  361.        
  362.         //right_bumper.init(&right_sensor_push);
  363.         //main boss;
  364.         boss->demo(300, 2);
  365.        
  366.         for (;;)
  367.         {
  368.        
  369.                 sensor.checkDistance();
  370.        
  371.                 boss->collision_sensor(&front_left, &engine, 0.6, -0.3, 0.0, 0.0);
  372.                 boss->collision_sensor(&front_right, &engine, 0.6, -0.3, 0.0, 0.0);
  373.                 boss->collision_sensor(&back_left, &engine, 0.6, -0.3, 0.0, 0.0);
  374.                 boss->collision_sensor(&back_right, &engine, 0.6, -0.3, 0.0, 0.0);
  375.                 boss->light(&Led_front_left,off,on,0.4f);
  376.                 boss->light(&Led_front_right, off, on, 0.4f);
  377.                 boss->light(&Led_back_left, off, on, 0.4f);
  378.                 boss->light(&Led_back_right, off, on, 0.4f);
  379.                 boss->battery_indicator(&Battery, &Led_red_battery, &Led_green_battery, &Led_yellow_battery);
  380.                
  381.                 //Led_red_battery->state(0);
  382.                  
  383.                
  384.                
  385.                 //Bluetooth.printf("Poziom naładowania baterii %2d V .\n", Battery->monitor_battery());
  386.         //      wait(1);
  387.                
  388.         }
  389. }

odpowiedź "robot"

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

captcha