Diff: main.cpp

Revision:

1:3d46a0660d05

Parent:

0:3fbb3fa4ff64

--- a/main.cpp	Fri Apr 12 06:58:18 2019 +0000
+++ b/main.cpp	Fri Apr 12 09:55:53 2019 +0000
@@ -14,21 +14,38 @@
 #include <cstdlib>
 #include <math.h>
 #include "mbed.h"
+#include "rtos.h"
 #include "Sensor.h"
 #include "Filtered_data.h"
 #include "DataStore.h"
 #include "ComplementaryFilter.h"
 #include "PID_caller.h"
 #include "Force_to_wing_angle.h"
+#include "Daan_Test1_maxon.h"
+#include "Move.h"
+
 //DigitalIn button(USER_BUTTON);
 DigitalOut led(LED2);
 using namespace std;
 
-/*
- * main.
- */
+RawSerial pc(SERIAL_TX,SERIAL_RX);
+CAN can(PB_8,PB_9);                                                             //NOG CHECKEN!!!
+
+EPOS epos1(1);
+EPOS epos2(2);
+EPOS epos4(4);
+
+Thread thread1;
+Thread thread2;
+Thread thread4;
+/* -----------------------------------------------------------------------------
+Main
+----------------------------------------------------------------------------- */
 int main(int argc, char** argv) {
-
+    pc.baud(921600);
+    can.frequency(250000);                                                      //NOG AANPASSEN IN EPOS COMPUTER PROGRAMMA ZODAT DE CAN FREQUENCY OP MOTORCONTROLLER OVEREENKOMT MET DIE VAN DE MASTER!! -> value 3
+    pc.printf("startup: \r\n"); 
+    
     // Maak de objecten.
     DataStore * ruwe_data = new DataStore();
     DataStore * filtered_data = new DataStore();
@@ -38,9 +55,9 @@
     Sensor * de_sensor = new Sensor();
     RuwDataFilter * filter = new RuwDataFilter();
     ComplementaryFilter * com_filter = new ComplementaryFilter();
-    PID_caller * PID =new PID_caller();
+    PID_caller * PID = new PID_caller();
     control::ForceToWingAngle * FtoW = new control::ForceToWingAngle();
-    
+    MOVE * moving = new MOVE();
 
     // De associaties, de verbindingen tussen de objecten.
     de_sensor->m_ruwe_state_data = ruwe_data;  // (1)
@@ -48,21 +65,59 @@
     filter->m_filtered_data = filtered_data;        // (3)
     com_filter->m_filtered_data = filtered_data; //(4)
     com_filter->m_complementary_data = complementary_data; //(5)
-    PID->m_complementary_data=complementary_data;  //(6)
-    PID->m_PID_data=pid_data; //(7)
-    FtoW->m_complementary_data=complementary_data; //(8)
-    FtoW->m_PID_data=pid_data; //(9)
-    FtoW->m_FtoW_data=FtoW_data; //(10)
+    PID->m_complementary_data = complementary_data;  //(6)
+    PID->m_PID_data = pid_data; //(7)
+    FtoW->m_complementary_data = complementary_data; //(8)
+    FtoW->m_PID_data = pid_data; //(9)
+    FtoW->m_FtoW_data = FtoW_data; //(10)
+    moving->m_FtoW_data = FtoW_data; //(11)
+    
+/* -----------------------------------------------------------------------------
+All three motors are going to home.
+----------------------------------------------------------------------------- */    
+    thread4.start(&epos4,&EPOS::Homing);
+    wait(15);
+    
+    thread2.start(&epos2,&EPOS::Homing);                                         //start de functie Homing
+    wait(15);   
+    //Thread::wait(1); 
+    
+    thread1.start(&epos1,&EPOS::Homing);                                         //start de functie Homing
+    wait(20);//Thread::wait(100); //was 100 
+    
+    thread4.join();
+    thread2.join();    
+    thread1.join();
+    wait(20);
+    
+/* -----------------------------------------------------------------------------
+All three motors are going in the startpositionmode.
+----------------------------------------------------------------------------- */    
+    thread1.start(&epos1,&EPOS::StartPositionMode);                                         
+    Thread::wait(1000);      //was 1000 
     
     
+    thread2.start(&epos2,&EPOS::StartPositionMode);                                         
+    Thread::wait(1000);      //was 1000 
+    
+    
+    thread4.start(&epos4,&EPOS::StartPositionMode);
+    Thread::wait(1000);
+    
+    thread4.join();
+    thread2.join();
+    thread1.join();
+    wait(20);
+        
     de_sensor->get_data();
     int N=0;
-    while (N < 100) {
+    while (N < 2) {
         led = !led;
         filter->FilterIt();
         com_filter->CalculateRealHeight();
         PID->PID_in();
         FtoW->MMA();
+        moving->Move();
         de_sensor->get_data();
     N=N+1;    
     }
@@ -77,6 +132,7 @@
     delete (PID);
     delete (com_filter);
     delete (FtoW);
+    delete (moving);
 
     return 0;
 }
\ No newline at end of file