API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 22nd, 2019
77
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Arduino 14.53 KB | None | 0 0
raw download clone embed print report
  1. #include <Arduino.h>
  2.  
  3. //ESC bibliotheken
  4. #include "esp32-hal-ledc.h"
  5. #define TIMER_WIDTH 16
  6. #include <Servo.h>
  7. Servo MotorL, MotorR;
  8. boolean richtingL, richtingR;
  9. int snelheidL, snelheidR;
  10.  
  11. double beginPunt[2] = {51.111634, 4.006755}; // Eerste parameter lengtegraad, tweede breedtegraad
  12. double eindPunt[2] = {51.111642, 4.006737};
  13. double hoekBoot;
  14. double hoekLocatie;
  15. boolean autopilot;
  16.  
  17. #include <TinyGPS++.h>
  18. TinyGPSPlus gps;
  19.  
  20. #include "Wire.h"
  21. #include <QMC5883L.h>
  22.  
  23. QMC5883L compass;
  24.  
  25. //Initialisatie van Timers:
  26.  
  27.   // ServoL
  28. hw_timer_t * servoL = NULL;
  29. volatile SemaphoreHandle_t timerServoL;
  30. portMUX_TYPE timerMuxServoL = portMUX_INITIALIZER_UNLOCKED;
  31. Servo ServoL;
  32.   // ServoR
  33. hw_timer_t * servoR = NULL;
  34. volatile SemaphoreHandle_t timerServoR;
  35. portMUX_TYPE timerMuxServoR = portMUX_INITIALIZER_UNLOCKED;
  36. Servo ServoR;
  37.  
  38.   //ElektromagneetL
  39. hw_timer_t * magnetL = NULL;
  40. volatile SemaphoreHandle_t timerMagnetL;
  41. portMUX_TYPE timerMuxMagnetL = portMUX_INITIALIZER_UNLOCKED;
  42.  
  43.   //ElektromagneetL
  44. hw_timer_t * magnetR = NULL;
  45. volatile SemaphoreHandle_t timerMagnetR;
  46. portMUX_TYPE timerMuxMagnetR = portMUX_INITIALIZER_UNLOCKED;
  47.  
  48. //Bibliotheken voor WebSocket
  49. #include <WiFi.h>
  50. #include <WiFiClientSecure.h>
  51.  
  52. #include <WebSocketsClient.h>
  53.  
  54. #include <esp_wifi.h>
  55.  
  56. //Definiëring van pin nummers:
  57. #define pinMotorL 19
  58. #define pinMotorR 23
  59. #define pinServoL 5
  60. #define pinServoR 18
  61. #define pinMagnetL 32
  62. #define pinMagnetR 33
  63.  
  64. const char* ssid = "Baitboat";//AP ssid
  65. const char* password = "Password";//AP password
  66.  
  67. TaskHandle_t Task1;
  68. TaskHandle_t GPSKompas;
  69.  
  70. WebSocketsClient webSocket;
  71.  
  72. void webSocketEvent(WStype_t type, uint8_t * payload, size_t length) {
  73.     switch(type) {
  74.         case WStype_DISCONNECTED:
  75.             Serial.printf("Verbinding verbroken!\n");
  76.       digitalWrite(2, LOW);
  77.             break;
  78.         case WStype_CONNECTED:
  79.             webSocket.sendTXT("Geconnecteerd!");
  80.       digitalWrite(2, HIGH);
  81.             break;
  82.         case WStype_TEXT:      
  83.       analysePS3(payload);
  84.       Serial.printf("Ontvangen tekst: %s\n", payload);  
  85.             break;
  86.     }
  87. }
  88.  
  89. void setup() {
  90.     Serial.begin(115200);
  91.   pinMode(2, OUTPUT);
  92.   Serial.println("Hoofd-serial started!");
  93.   Serial.println("Ontwikkelen van kern 0 ...");
  94.   xTaskCreatePinnedToCore(
  95.                     Task1code,   /* Task function. */
  96.                     "Task1",     /* name of task. */
  97.                     10000,       /* Stack size of task */
  98.                     NULL,        /* parameter of the task */
  99.                     2,           /* priority of the task */
  100.                     &Task1,      /* Task handle to keep track of created task */
  101.                     0);
  102.   /*xTaskCreatePinnedToCore(
  103.                     GPSKompascode,  
  104.                     "GPSKompas",    
  105.                     10000,      
  106.                     NULL,        
  107.                     1,          
  108.                     &GPSKompas,      
  109.                     0);*/
  110.   Serial.println("Kern 0 succesvol ontwikkeld!");
  111.  
  112.   Serial.println("Speedcontrollers verbinden ...");  
  113.   /*MotorL.setPeriodHertz(50);
  114.   MotorL.attach(pinMotorL, 1000, 2000);
  115.   richtingL = true;
  116.   snelheidL = 1500;
  117.  
  118.   MotorR.setPeriodHertz(50);
  119.   MotorR.attach(pinMotorR, 1000, 2000);
  120.   richtingR = true;
  121.   snelheidR = 1500;*/
  122.   /*ledcSetup(1, 50, TIMER_WIDTH);
  123.   ledcAttachPin(pinMotorL, 1);
  124.   snelheidL = 1500;
  125.   snelheidR = 1500;
  126.   ledcSetup(2, 50, TIMER_WIDTH);
  127.   ledcAttachPin(pinMotorR, 2);*/
  128.   MotorL.attach(pinMotorL);
  129.   MotorL.writeMicroseconds(1500);
  130.   snelheidL = 1500;
  131.   Serial.println(snelheidL);
  132.   MotorR.attach(pinMotorR);
  133.   MotorR.writeMicroseconds(1500);
  134.   snelheidR = 1500;
  135.   Serial.println(snelheidR);
  136.   Serial.println("Speedcontrollers succesvol verbonden!");
  137.  
  138.   Serial.println("Servo's verbinden ...");
  139.   initServoL();
  140.   initServoR();
  141.   Serial.println("Servo's succesvol verbonden!");
  142.    
  143.   Serial.println("Elektromagneten verbinden ...");
  144.   initMagnetL();
  145.   initMagnetR();
  146.   Serial.println("Elektromagneten succesvol verbonden!");
  147. }
  148.  
  149. void GPSKompascode( void * pvParameters ){
  150.   /*Serial2.begin(9600, SERIAL_8N1, 16, 17);
  151.   Serial.println("Serial2 succesvol gestart!");
  152.  
  153.   Wire.begin();
  154.   compass.init();
  155.   compass.setSamplingRate(10);
  156.  
  157.   for(;;){
  158.     while (Serial2.available() > 0){
  159.       gps.encode(Serial2.read());
  160.       if (gps.location.isUpdated()){
  161.         Serial.print("Latitude= ");
  162.         Serial.print(gps.location.lat(), 6);
  163.         Serial.print(" Longitude= ");
  164.         Serial.print(gps.location.lng(), 6);
  165.         Serial.print(" GPS-satelieten: ");
  166.         Serial.println(gps.satellites.value());
  167.       }  
  168.     }
  169.     int heading = compass.readHeading();
  170.     if(heading==0) {
  171.       /* Still calibrating, so measure but don't print
  172.     } else {
  173.       Serial.println(heading);
  174.       hoekBoot = (double) heading;
  175.     }
  176.   }*/
  177.   /*for(;;){
  178.     //MotorL.write(snelheidL);
  179.     //MotorR.write(snelheidR);
  180.     Serial.println("SnelheidL: " + snelheidL);
  181.     Serial.println("SnelheidR: " + snelheidR);
  182.     vTaskDelay(2000/10);
  183.   }*/
  184. }
  185.  
  186. void Task1code( void * pvParameters ){
  187.   WiFi.mode( WIFI_STA );
  188.   esp_wifi_set_protocol( WIFI_IF_STA, WIFI_PROTOCOL_LR );
  189.          
  190.   WiFi.begin(ssid, password);
  191.   Serial.println("Verbinding maken met HUB ...");
  192.   while (WiFi.status() != WL_CONNECTED){
  193.     delay(500);
  194.     Serial.print(".");
  195.   }
  196.   Serial.println("");
  197.   Serial.println("Geconnecteerd met HUB!");
  198.   Serial.print("IP address: ");
  199.   Serial.println(WiFi.localIP());
  200.   Serial.println("Websocket-verbinding starten ...");
  201.   webSocket.begin("192.168.4.1", 81, "/");
  202.   webSocket.onEvent(webSocketEvent);
  203.   webSocket.setReconnectInterval(5000);
  204.   Serial.println("Websocket-verbinding gestart!");
  205.   for(;;){
  206.     webSocket.loop();
  207.     vTaskDelay(1000/10);
  208.   }
  209. }
  210.  
  211. void loop() {
  212.   MotorL.writeMicroseconds(snelheidL);
  213.   MotorR.writeMicroseconds(snelheidR);  
  214.   delay(200);
  215. }
  216.  
  217. void analysePS3(uint8_t * payload){
  218.   String text = (char*) payload;
  219.   //Linkse motor
  220.   if (text.substring(0,3) == "ML>"){
  221.     text.remove(0,3);    
  222.     if (text.substring(0,3) == "HLD"){
  223.       snelheidL = 1500;
  224.       return;
  225.     }
  226.     if (text.substring(0,3) == "FWD"){
  227.       //MotorL.setDirection(ESC::FORWARD);
  228.       richtingL = true;
  229.       return;
  230.     }
  231.     if (text.substring(0,3) == "BWD"){
  232.       //MotorL.setDirection(ESC::BACKWARD);
  233.       richtingL = false;
  234.       return;
  235.     }
  236.     //MotorL.//setSpeed(text.toInt());
  237.     int snelheid = map(text.toInt(), 0, 500, 0, 300);
  238.     snelheidL = richtingL ? (1500 + snelheid) : (1500 - snelheid);
  239.     return;
  240.   }
  241.   //Rechtse motor
  242.   if (text.substring(0,3) == "MR>"){
  243.     text.remove(0,3);    
  244.     if (text.substring(0,3) == "HLD"){
  245.       snelheidR = 1500;
  246.       return;
  247.     }
  248.     if (text.substring(0,3) == "FWD"){
  249.       //MotorR.setDirection(ESC::FORWARD);
  250.       richtingR = true;
  251.       return;
  252.     }
  253.     if (text.substring(0,3) == "BWD"){
  254.       //MotorR.setDirection(ESC::BACKWARD);
  255.       richtingR = false;
  256.       return;
  257.     }
  258.     //MotorR.//setSpeed(text.toInt());
  259.     int snelheid = map(text.toInt(), 0, 500, 0, 300);
  260.     snelheidR = richtingL ? (1500 + snelheid) : (1500 - snelheid);
  261.     return;
  262.   }
  263.   //Linkse servo
  264.   if (text.substring(0,2) == "L2"){
  265.     ServoL.write(60);
  266.     timerRestart(servoL);
  267.     return;
  268.   }
  269.   //Rechtse servo
  270.   if (text.substring(0,2) == "R2"){
  271.     ServoR.write(60);
  272.     timerRestart(servoR);
  273.     return;
  274.   }
  275.   //Linkse elektromagneet
  276.   if (text.substring(0,2) == "L1"){
  277.     digitalWrite(pinMagnetL, HIGH);
  278.     timerRestart(magnetL);
  279.     return;
  280.   }
  281.   //Linkse elektromagneet
  282.   if (text.substring(0,2) == "R1"){
  283.     digitalWrite(pinMagnetR, HIGH);
  284.     timerRestart(magnetR);
  285.     return;
  286.   }  
  287. }
  288.  
  289. void IRAM_ATTR onServoL(){
  290.   portENTER_CRITICAL_ISR(&timerMuxServoL);
  291.   portEXIT_CRITICAL_ISR(&timerMuxServoL);
  292.   //Code voor timer
  293.   ServoL.write(0);
  294.   timerStop(servoL);
  295. }
  296.  
  297. void initServoL(){
  298.   ServoL.attach(pinServoL);
  299.   timerServoL = xSemaphoreCreateBinary();
  300.   servoL = timerBegin(0, 80, true);
  301.   timerAttachInterrupt(servoL, &onServoL, true);
  302.   timerAlarmWrite(servoL, 1500000, true);
  303.   timerAlarmEnable(servoL);
  304. }
  305.  
  306. void IRAM_ATTR onServoR(){
  307.   portENTER_CRITICAL_ISR(&timerMuxServoR);
  308.   portEXIT_CRITICAL_ISR(&timerMuxServoR);
  309.   //Code voor timer
  310.   ServoR.write(0);
  311.   timerStop(servoR);
  312. }
  313.  
  314. void initServoR(){
  315.   ServoR.attach(pinServoR);
  316.   timerServoR = xSemaphoreCreateBinary();
  317.   servoR = timerBegin(1, 80, true);
  318.   timerAttachInterrupt(servoR, &onServoR, true);
  319.   timerAlarmWrite(servoR, 1500000, true);
  320.   timerAlarmEnable(servoR);
  321. }
  322.  
  323. void IRAM_ATTR onMagnetL(){
  324.   portENTER_CRITICAL_ISR(&timerMuxMagnetL);
  325.   portEXIT_CRITICAL_ISR(&timerMuxMagnetL);
  326.   //Code voor timer
  327.   digitalWrite(pinMagnetL, LOW);
  328.   timerStop(magnetL);
  329. }
  330.  
  331. void initMagnetL(){
  332.   pinMode(pinMagnetL, OUTPUT);
  333.   timerMagnetL = xSemaphoreCreateBinary();
  334.   magnetL = timerBegin(2, 80, true);
  335.   timerAttachInterrupt(magnetL, &onMagnetL, true);
  336.   timerAlarmWrite(magnetL, 1000000, true);
  337.   timerAlarmEnable(magnetL);
  338. }
  339.  
  340. void IRAM_ATTR onMagnetR(){
  341.   portENTER_CRITICAL_ISR(&timerMuxMagnetR);
  342.   portEXIT_CRITICAL_ISR(&timerMuxMagnetR);
  343.   //Code voor timer
  344.   digitalWrite(pinMagnetR, LOW);
  345.   timerStop(magnetR);
  346. }
  347.  
  348. void initMagnetR(){
  349.   pinMode(pinMagnetR, OUTPUT);
  350.   timerMagnetR = xSemaphoreCreateBinary();
  351.   magnetR = timerBegin(3, 80, true);
  352.   timerAttachInterrupt(magnetR, &onMagnetR, true);
  353.   timerAlarmWrite(magnetR, 1000000, true);
  354.   timerAlarmEnable(magnetR);
  355. }
  356.  
  357. /*void //setSpeed(ESC motor, int snelheid){
  358.   //motor.//setSpeed(snelheid);  
  359. }*/
  360.  
  361. void autoPilot(){
  362.   double hoekVerschil;
  363.   double afstandVerschil;
  364.   boolean richting; //zal "true" zijn bij CW, en "false" bij CCW
  365.   int motorL;
  366.   int motorR;
  367.   int vaarSnelheid = 350; //Vaarsnelheid van de motor, waarde tussen 0 en 500
  368.   int correctieSnelheid1; //Snelheid waaraan de boot boot zal draaien voor hoekverschil-compensatie
  369.   int correctieSnelheid2; //Snelheid dat een motor meer/minder zal draaien voor rechte lijn
  370.  
  371.   hoekVerschil = gradenVerschil((int)hoekLocatie, (int)hoekBoot);
  372.   hoekLocatie = hoek(beginPunt, eindPunt);
  373.  
  374.   while(autopilot){
  375.     //Eerst moeten we de boot in de juiste hoek zetten zodat we +- in een rechte lijn kunnen varen
  376.     while (hoekVerschil > 15){ //Als het hoekverschil groter is dan 15°, dan ...
  377.       //Kompas aflezen ... Nog toe te voegen
  378.      
  379.       hoekVerschil = gradenVerschil((int)hoekLocatie, (int)hoekBoot);
  380.      
  381.       //Bepalen van snelheid waaraan er één motor moet draaien
  382.       correctieSnelheid1 = map(hoekVerschil, 180, 0, 250, 100);
  383.      
  384.       //Snelheid van die ene motor bijwerken
  385.       //setSpeed(richting ? MotorL : MotorR , correctieSnelheid1);      
  386.      
  387.       delay(100);
  388.     }
  389.    
  390.     // Nadat de boot +- in een rechte lijn staat, kan de boot beginnen varen.
  391.     while (afstandVerschil > 5){ //Als de afstand tussen de boot en de aankomst groter is dan 5m, dan ...
  392.       //GPS-Locatie bijwerken ... Nog toe te voegen
  393.       //Kompas bijwerken ... Nog toe te voegen
  394.  
  395.       //Bijwerken van de afstandsverschillen en hoekverschillen
  396.       afstandVerschil = afstand(beginPunt, eindPunt);
  397.       hoekLocatie = hoek(beginPunt, eindPunt);
  398.       hoekVerschil = gradenVerschil((int)hoekLocatie, (int)hoekBoot);
  399.  
  400.       //Berekenen van correctiesnelheid om in een zo recht mogelijke lijn te varen
  401.       correctieSnelheid2 = map(hoekVerschil, 90, 0, 100, 0);
  402.  
  403.       //Snelheid van de motoren bijwerken
  404.       //setSpeed(MotorL, richting ? (vaarSnelheid - correctieSnelheid2) : vaarSnelheid + correctieSnelheid2);
  405.       //setSpeed(MotorR, richting ? (vaarSnelheid + correctieSnelheid2) : vaarSnelheid - correctieSnelheid2);
  406.  
  407.       delay(200);
  408.     }
  409.   }
  410. }
  411.  
  412. /*
  413.   Bron: https://www.movable-type.co.uk/scripts/latlong.html
  414.   Op deze site wordt de formule van Haversine uitgelegd.
  415.   Deze formule is omgezet naar Arduino-code in deze functie.
  416. */
  417. double afstand(double loc1[], double loc2[]){
  418.   double straalAarde, radLengte1, radLengte2, deltaLengte, deltaBreedte, a, c, d; //Declareren variabelen
  419.   straalAarde = 6371000; // Straal van aarde in meter
  420.   radLengte1 = gradenNaarRad(loc1[0]); // Omzetten van graden naar radialen
  421.   radLengte2 = gradenNaarRad(loc2[0]); // Omzetten van graden naar radialen
  422.   deltaLengte = gradenNaarRad(loc2[0] - loc1[0]); // Verschil van lengtegraden omzetten naar radialen
  423.   deltaBreedte = gradenNaarRad(loc2[1] - loc1[1]); // Verschil van breedtegraden omzetten naar radialen
  424.   a = sin(deltaLengte/2)*sin(deltaLengte/2) +
  425.       cos(radLengte1) * cos(radLengte2) *
  426.       sin(deltaBreedte/2) * sin(deltaBreedte/2);    
  427.   c = 2 * atan2(sqrt(a), sqrt(1-a));
  428.   d = straalAarde * c;
  429.   return d;  
  430. }
  431.  
  432. /*
  433.  * Formule: radialen = atan2(sin(deltaLengte).cos(lat2), cos(lat1).sin(lat2)− sin(lat1).cos(lat2).cos(deltaLengte))
  434.  * Het probleem is wel dat bij de wiskunde de X-as als referentie wordt genomen, waarbij bij een kompas het noorden (Y-as)
  435.  * als referentie wordt genomen. Hierdoor wordt de uitgekomen graden eerst nog geanalyseerd door de functie
  436.  * analyseGraden().
  437.  */
  438. double hoek(double loc1[], double loc2[]){
  439.   double deltaLengte, y, x, graden;
  440.   deltaLengte = gradenNaarRad(loc2[1] - loc1[1]);
  441.   y = sin(deltaLengte) * cos(gradenNaarRad(loc2[0]));
  442.   x = cos(gradenNaarRad(loc1[0])) * sin(gradenNaarRad(loc2[0])) -
  443.       sin(gradenNaarRad(loc1[0])) * cos(gradenNaarRad(loc2[0])) * cos(deltaLengte);
  444.   graden = radNaarGraden(atan2(x, y));
  445.   return analyseGraden(graden);
  446. }
  447. /*
  448.  * Functie dat de referentie verschuift van de X-as naar de Y-as.
  449.  */
  450. double analyseGraden(double graden){
  451.   if (graden > -90 && graden < 90){
  452.     graden = map(graden, -90, 90, 180, 0);    
  453.   }
  454.   else if (graden <= -90){
  455.     graden = map(graden, -180, -90, 270, 180);    
  456.   }
  457.   else if (graden >= 90){
  458.     graden = map(graden, 180, 90, 270, 360);
  459.   }  
  460.   if (graden == 360){
  461.     graden = 0;
  462.   }
  463.   return graden;
  464. }
  465.  
  466. /*
  467.  * Functie om de kleinste hoek tussen 2 graden in een cirkel te bepalen.
  468.  */
  469. double gradenVerschil(int g1, int g2){
  470.   double verschil = abs(g2 - g1) % 360;    
  471.   return verschil > 180 ? 360 - verschil : verschil;
  472. }
  473.  
  474. /*
  475.  * Functie om te bepalen wat de meest efficiënte richting is, met de klok mee (clockWise)
  476.  * of tegen de klok in (counterClockWise). De functie zal "true" zijn als het clockWise is.
  477.  */
  478. boolean clockWise(int g1, int g2){
  479.   return (g2 - g1 + 360) % 360 > 180;
  480. }
  481.  
  482. //Functie om graden om te zetten naar radialen
  483. double gradenNaarRad(double graden){
  484.   return(graden * PI /180);
  485. }
  486.  
  487. //Functie om radialen om te zetten naar graden
  488. double radNaarGraden(double radialen){
  489.   return(radialen * 180.0 / PI);
  490. }
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!