API tools faq
paste
Advertisement
Guest User

øwaoepfkøapewkof

a guest
Sep 21st, 2018
67
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 11.67 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.     Name:       Robot.ino
  3.     Created:    29-08-2018 13:51:56
  4.     Author:    Adam Tallouzi, Christoffer Lyholm & Nicolai Lyholm
  5. */
  6.  
  7. #include <MeMegaPi.h>
  8. #include <Wire.h>
  9. #include <SoftwareSerial.h>
  10. #include <math.h>
  11.  
  12. MeEncoderOnBoard Encoder_1(SLOT2);
  13. MeEncoderOnBoard Encoder_2(SLOT1);
  14. MeMegaPiDCMotor Motor_1(PORT2B);
  15. MeMegaPiDCMotor Motor_2(PORT1B);
  16. MeMegaPiDCMotor Clamp(PORT4B);
  17. MeMegaPiDCMotor Arm(PORT3B);
  18. MeLineFollower LineFollower(PORT_6);
  19. MeGyro Gyro;
  20.  
  21. byte ReceiveDataBuffer[16];
  22. byte TransmitBuffer[16];
  23. byte cnt = 0;
  24. int motorSpeedLimit = 200;
  25. int timer = 0;
  26.  
  27. void isr_process_encoder1(void)
  28. {
  29.     if (digitalRead(Encoder_1.getPortB()) == 0)
  30.     {
  31.         Encoder_1.pulsePosMinus();
  32.     }
  33.     else
  34.     {
  35.         Encoder_1.pulsePosPlus();
  36.     }
  37. }
  38.  
  39. void isr_process_encoder2(void)
  40. {
  41.     if (digitalRead(Encoder_2.getPortB()) == 0)
  42.     {
  43.         Encoder_2.pulsePosMinus();
  44.     }
  45.     else
  46.     {
  47.         Encoder_2.pulsePosPlus();
  48.     }
  49. }
  50.  
  51. void setup()
  52. {
  53.     attachInterrupt(Encoder_1.getIntNum(), isr_process_encoder1, RISING);
  54.     attachInterrupt(Encoder_2.getIntNum(), isr_process_encoder2, RISING);
  55.     Serial.begin(115200);
  56.     Serial3.begin(115200);
  57.     Gyro.begin();
  58.  
  59.     TCCR1A = _BV(WGM10);
  60.     TCCR1B = _BV(CS11) | _BV(WGM12);
  61.  
  62.     TCCR2A = _BV(WGM21) | _BV(WGM20);
  63.     TCCR2B = _BV(CS21);
  64.  
  65.     Encoder_1.setPulse(7);
  66.     Encoder_2.setPulse(7);
  67.     Encoder_1.setRatio(26.9);
  68.     Encoder_2.setRatio(26.9);
  69.  
  70.     Encoder_1.setPosPid(1.8, 0, 1.2);
  71.     Encoder_2.setPosPid(1.8, 0, 1.2);
  72.     Encoder_1.setSpeedPid(0.18, 0, 0);
  73.     Encoder_2.setSpeedPid(0.18, 0, 0);
  74. }
  75.  
  76. void loop()
  77. {
  78.     controlArm(100, 3000);
  79.     delay(1000);
  80.     driveEncoder(62 - 18, motorSpeedLimit, 1);
  81.     controlArm(-100, 3000 - 500);
  82.     delay(1000);
  83.     controlClamp(100);
  84.     controlArm(100, 1500); //Armen kører ca. halv-vejs op med dåsen (Lav eventuelt et timer for-loop for controlArm)
  85.     delay(1000);
  86.     turn(-90);
  87.     driveEncoder(136, motorSpeedLimit, 1);
  88.     turn(-90);
  89.     driveEncoder(23, motorSpeedLimit, 1);
  90.     turn(90);
  91.     driveEncoder(58, motorSpeedLimit, 1); //Eventuelt mindre afstand
  92.     controlArm(-100, 1500 - 500);
  93.     controlClamp(-100);
  94.     controlArm(100, 3000); //Løft arm til øverst mulige position
  95.     while (true);
  96.  
  97. }
  98.  
  99. //En negativ vinkel er med uret, positiv er mod uret, skal være mellem -180 - 180
  100. void turn(int Angle)
  101. {
  102.     Gyro.begin();
  103.     int tidTurn = 10 * Angle;
  104.  
  105.  
  106.     for (timer = 0; timer < tidTurn; timer++);
  107.     {
  108.  
  109.         if (Angle < 0)
  110.         {
  111.             Motor_1.run(180);
  112.             Motor_2.run(180);
  113.         }
  114.         else
  115.         {
  116.             Motor_1.run(-180);
  117.             Motor_2.run(-180);
  118.         }
  119.  
  120.         while (abs(Gyro.getAngleZ()) <= abs(Angle) - 1)
  121.         {
  122.             Gyro.update();
  123.             if (abs(Gyro.getAngleZ()) >= abs(Angle) - 1)
  124.             {
  125.                 Motor_1.stop();
  126.                 Motor_2.stop();
  127.             }
  128.             delay(5);
  129.         }
  130.  
  131.     }
  132.     timer = 0;
  133. }
  134.  
  135. void LineFollow()
  136. /* Værdi   Sensor1 Sensor2
  137.     0       Sort    Sort
  138.     1       Sort    Hvid
  139.     2       Hvid    Sort
  140.     3       Hvid    Hvid */
  141. {
  142.     if (LineFollower.readSensors() == 3)
  143.     {
  144.         Motor_1.run(40);
  145.         Motor_2.run(-150);
  146.     }
  147.     if (LineFollower.readSensors() == 1)
  148.     {
  149.         Motor_1.run(100);
  150.         Motor_2.run(100);
  151.     }
  152.     if (LineFollower.readSensors() == 0)
  153.     {
  154.         Motor_1.run(150);
  155.         Motor_2.run(-40);
  156.     }
  157.  
  158. }
  159.  
  160. void controlArm(int speed, int duration)
  161. {
  162.     Arm.run(speed);
  163.     delay(duration);
  164.     Arm.stop();
  165. }
  166.  
  167.  
  168. //åbner ved negativ værdi, lukker ved positiv værdi
  169. void controlClamp(int speed)
  170. {
  171.     Clamp.run(speed);
  172.     delay(2500);
  173.     Clamp.stop();
  174. }
  175.  
  176. void driveEncoder(float distance, float speed, int sign)
  177. {
  178.     Encoder_1.reset(SLOT2);
  179.     Encoder_2.reset(SLOT1);
  180.  
  181.     Encoder_1.moveTo(sign*(-((distance * 1.1 * 180) / (3.15*PI))), speed);
  182.     Encoder_2.moveTo(sign*(distance * 1.1 * 180) / (3.15*PI), speed);
  183.     int tidDrive = 0;
  184.     if (distance > 100)
  185.     {
  186.         tidDrive = distance / speed * 200;
  187.     }
  188.     tidDrive = 200 * distance / speed;
  189.  
  190.     for (timer = 0; timer < tidDrive; timer++)
  191.     {
  192.         Encoder_1.loop();
  193.         Encoder_2.loop();
  194.         delay(100);
  195.     }
  196.     timer = 0;
  197.  
  198.  
  199. }
  200.  
  201. void isDataRecieved()
  202. {
  203.     byte readData = 0;
  204.     byte nof_data = 0;
  205.     cnt = 0;
  206.     if (Serial3.available() != (int)0)
  207.     {
  208.         Serial.println("Data recieved");
  209.         while (Serial3.available() != (int)0)
  210.         {
  211.             Serial.println(Serial3.available());
  212.             readData = Serial3.read();
  213.             ReceiveDataBuffer[cnt] = readData;
  214.             cnt++;
  215.             nof_data++;
  216.             delay(1);
  217.         }
  218.         DecodeData(nof_data);
  219.     }
  220. }
  221.  
  222. void DecodeData(byte nofData)
  223. {
  224.     for (int cnt = 0; cnt < nofData; cnt++)
  225.     {
  226.         TransmitBuffer[cnt] = ReceiveDataBuffer[cnt];
  227.     }
  228.     SendData(nofData);
  229.     switch (ReceiveDataBuffer[0])
  230.     {
  231.         //manual
  232.     case 1:
  233.         switch (ReceiveDataBuffer[1])
  234.         {
  235.             //motorer
  236.         case 1:
  237.             switch (ReceiveDataBuffer[2])
  238.             {
  239.             case 1:
  240.                 int motorSpeed1;
  241.                 int motorSpeed2;
  242.  
  243.                 //for at undgå fejl i kompilering, ændres variablerne seperat
  244.                 if (ReceiveDataBuffer[3] > 127)
  245.                 {
  246.                     motorSpeed1 = round((ReceiveDataBuffer[3] - 256) / 100 * motorSpeedLimit);
  247.                 }
  248.                 else
  249.                 {
  250.                     motorSpeed1 = round(ReceiveDataBuffer[3] / 100 * motorSpeedLimit);
  251.                 }
  252.  
  253.                 if (ReceiveDataBuffer[4] > 127)
  254.                 {
  255.                     motorSpeed2 = round((ReceiveDataBuffer[4] - 256) / 100 * motorSpeedLimit);
  256.                 }
  257.                 else
  258.                 {
  259.                     motorSpeed2 = round(ReceiveDataBuffer[4] / 100 * motorSpeedLimit);
  260.                 }
  261.  
  262.                 Motor_1.run(motorSpeed1);
  263.                 Motor_2.run(motorSpeed2);
  264.                 break;
  265.  
  266.             case 2:
  267.                 Motor_1.stop();
  268.                 Motor_2.stop();
  269.                 break;
  270.             }
  271.             break;
  272.  
  273.         case 2:
  274.             switch (ReceiveDataBuffer[2])
  275.             {
  276.             case 1:
  277.                 Arm.run(100);
  278.                 break;
  279.  
  280.             case 2:
  281.                 Arm.run(-100);
  282.                 break;
  283.  
  284.             case 3:
  285.                 Arm.stop();
  286.                 break;
  287.             }
  288.             break;
  289.             //clamp
  290.         case 3:
  291.             switch (ReceiveDataBuffer[2])
  292.             {
  293.             case 1:
  294.                 Clamp.run(100);
  295.                 break;
  296.  
  297.             case 2:
  298.                 Clamp.run(-100);
  299.                 break;
  300.  
  301.             case 3:
  302.                 Clamp.stop();
  303.                 break;
  304.             }
  305.             break;
  306.         }
  307.         //auto
  308.     case 2:
  309.         switch (ReceiveDataBuffer[1])
  310.         {
  311.             /*case 1:
  312.                 controlArm(100, 3000);
  313.                 driveEncoder(57, motorSpeedLimit);
  314.                 controlArm(-100, 3000 - 500);
  315.                 //controlClamp();
  316.                 controlArm(100, 1500); //Armen kører ca. halv-vejs op med dåsen (Lav eventuelt et timer for-loop for controlArm)
  317.                 //Drive encoder bagudover (Mangler mål)
  318.                 turn(-90);
  319.                 driveEncoder(136, motorSpeedLimit);
  320.                 turn(-90);
  321.                 driveEncoder(28, motorSpeedLimit);
  322.                 turn(90);
  323.                 driveEncoder(56, motorSpeedLimit); //Eventuelt mindre afstand
  324.                 controlArm(-100, 1500 - 500);
  325.                 //controlClamp (åben clamp for at slippe dåse)
  326.                 controlArm(100, 3000); //Løft arm til øverst mulige position
  327.                 break;
  328.  
  329.             case 2:
  330.                 controlArm(100, 3000);
  331.                 driveEncoder(42, motorSpeedLimit);
  332.                 turn(-90);
  333.                 driveEncoder(86, motorSpeedLimit);
  334.                 turn(-90);
  335.                 driveEncoder(23, 150); //Træk afstand fra for at opsamle tønde
  336.                 controlArm(-100, 3000 - 500); //Arm kører helt ned
  337.                 //clampControl() Holder om dåse
  338.                 controlArm(100, 1500); //Arm kører halvt op med dåse
  339.                 driveEncoder(-23, 150); //Træk samme afstand fra som forrige drive funktion
  340.                 turn(90);
  341.                 driveEncoder(55, motorSpeedLimit);
  342.                 turn(-90);
  343.                 driveEncoder(28, motorSpeedLimit);
  344.                 turn(90);
  345.                 driveEncoder(56, motorSpeedLimit); //Træk afstand fra for at placere dåse
  346.                 controlArm(-100, 1500 - 500);
  347.                 //controlClamp() Åben clamp for at slippe dåse
  348.                 controlArm(100, 3000); //Kør arm til højeste position
  349.                 break;
  350.  
  351.             case 3:
  352.                 controlArm(100, 3000);
  353.                 driveEncoder(42, motorSpeedLimit);
  354.                 turn(-90);
  355.                 driveEncoder(80, motorSpeedLimit);
  356.                 turn(-90);
  357.                 driveEncoder(74, motorSpeedLimit);
  358.                 turn(90);
  359.                 driveEncoder(28, 150); // Træk afstand fra for opsamling af dåse
  360.                 controlArm(-100, 3000 - 500);
  361.                 //controlClamp()
  362.                 controlArm(100, 1500);
  363.                 turn(90);
  364.                 driveEncoder(9, 250);
  365.                 turn(-90);
  366.                 driveEncoder(18, 150);
  367.                 turn(90);
  368.                 driveEncoder(65, motorSpeedLimit);
  369.                 turn(-90);
  370.                 driveEncoder(42, motorSpeedLimit);
  371.                 turn(-90);
  372.                 driveEncoder(28, motorSpeedLimit);
  373.                 turn(90);
  374.                 driveEncoder(56, motorSpeedLimit); //træk afstand fra for placering af dåse
  375.                 controlArm(-100, 1500 - 500);
  376.                 //controlClamp()
  377.                 controlArm(100, 3000);
  378.                 break;
  379.  
  380.             case 4:
  381.                 controlArm(100, 3000);
  382.                 driveEncoder(42, motorSpeedLimit);
  383.                 turn(-90);
  384.                 driveEncoder(80, motorSpeedLimit);
  385.                 turn(-90);
  386.                 driveEncoder(68, motorSpeedLimit);
  387.                 turn(90);
  388.                 driveEncoder(36, motorSpeedLimit);
  389.                 turn(-90);
  390.                 driveEncoder(5, 150);
  391.                 turn(90);
  392.                 driveEncoder(38, motorSpeedLimit); //Træk afstand fra for opsamling af dåse
  393.                 controlArm(-100, 3000 - 500);
  394.                 //controlClamp()
  395.                 controlArm(100, 1500);
  396.                 turn(90);
  397.                 driveEncoder(28, motorSpeedLimit);
  398.                 turn(-90);
  399.                 driveEncoder(70, motorSpeedLimit); //træk afstand fra for placering af dåse
  400.                 controlArm(-100, 1500 - 500);
  401.                 //controlClamp()
  402.                 controlArm(100, 3000);
  403.                 break;
  404.  
  405.             case 5: //øverste tønde
  406.                 controlArm(100, 3000);
  407.                 driveEncoder(42, motorSpeedLimit);
  408.                 turn(-90);
  409.                 driveEncoder(162, motorSpeedLimit);
  410.                 turn(90);
  411.                 driveEncoder(11, motorSpeedLimit); //Træk afstand fra for opsamling af dåse
  412.                 controlArm(-100, 2000 - 500);
  413.                 //controlClamp()
  414.                 controlArm(100, 1500);
  415.                 driveEncoder(-47, motorSpeedLimit); //Træk samme afstand fra som forrige drive funktion
  416.                 turn(-90);
  417.                 driveEncoder(30, motorSpeedLimit); //Træk afstand fra for placering af dåse
  418.                 controlArm(-100, 1500 - 500);
  419.                 //controlClamp()
  420.                 controlArm(100, 3000);
  421.                 break;
  422.  
  423.             case 6: //nederste tønde
  424.                 controlArm(100, 3000);
  425.                 driveEncoder(42, motorSpeedLimit);
  426.                 turn(-90);
  427.                 driveEncoder(162, motorSpeedLimit);
  428.                 turn(90);
  429.                 driveEncoder(11, motorSpeedLimit); //Træk afstand fra for opsamling af dåse
  430.                 controlArm(-100, 3000 - 500);
  431.                 //controlClamp()
  432.                 controlArm(100, 1500);
  433.                 driveEncoder(-47, motorSpeedLimit); //Træk samme afstand fra som forrige drive funktion
  434.                 turn(-90);
  435.                 driveEncoder(30, motorSpeedLimit); //Træk afstand fra for placering af dåse
  436.                 controlArm(-100, 1500 - 500);
  437.                 //controlClamp()
  438.                 controlArm(100, 3000);
  439.                 break;
  440.  
  441.             case 7: //øverste tønde
  442.                 break;
  443.  
  444.             case 8: //nederste tønde
  445.                 controlArm(100, 3000);
  446.                 driveEncoder(42, motorSpeedLimit);
  447.                 turn(-90);
  448.                 driveEncoder(173, motorSpeedLimit);
  449.                 turn(90);
  450.                 driveEncoder(11, motorSpeedLimit); //Træk afstand fra for opsamling af dåse
  451.                 controlArm(-100, 3000 - 500);
  452.                 //controlClamp()
  453.                 controlArm(100, 1500);
  454.                 driveEncoder(-47, motorSpeedLimit); //Træk samme afstand fra som forrige drive funktion
  455.                 turn(-90);
  456.                 driveEncoder(19, motorSpeedLimit); //Træk afstand fra for placering af dåse
  457.                 controlArm(-100, 1500 - 500);
  458.                 //controlClamp()
  459.                 controlArm(100, 3000);
  460.                 break;
  461.  
  462.             case 9:
  463.                 controlArm(100, 3000);
  464.                 driveEncoder(42, motorSpeedLimit);
  465.                 turn(-90);
  466.                 driveEncoder(136, motorSpeedLimit);
  467.                 turn(-90);
  468.                 //driveEncoder(?,motorSpeedLimit); (kender ikke afstand)
  469.                 turn(90);
  470.                 driveEncoder(30, motorSpeedLimit);
  471.                 turn(-90);
  472.                 driveEncoder(25, motorSpeedLimit);
  473.                 turn(90);
  474.                 driveEncoder(24, motorSpeedLimit);
  475.                 turn(-45);
  476.                 //driveEncoder(?, motorSpeedLimit); (Kender ikke afstand, træk afstand fra for opsamling af dåse)
  477.                 controlArm(-100, 3000 - 500);
  478.                 //controlClamp
  479.                 controlArm(100, 1500);
  480.                 //driveEncoder(?, motorSpeedLimit); (Modsat afstand af forrige drive funktion)
  481.                 turn(45);
  482.                 driveEncoder(27, motorSpeedLimit);
  483.                 turn(-90);
  484.                 driveEncoder(6, 150); //Træk afstand fra for placering af dåse
  485.                 controlArm(-100, 1500 - 500);
  486.                 //controlClamp()
  487.                 controlArm(100, 3000);
  488.                 break;
  489.  
  490.             case 10: //cancel
  491.                 break;
  492.  
  493.                 break;
  494.                 */
  495.         }
  496.  
  497.     }
  498. }
  499.  
  500. void SendData(byte nofData) {
  501.     for (int cnt = 0; cnt < nofData; cnt++)
  502.     {
  503.         Serial3.write(TransmitBuffer[cnt]);
  504.         delay(1);
  505.     }
  506. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!