File content as of revision 1:3d46a0660d05:

/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/* 
 * File:   main.cpp
 * Author: kwieg
 *
 * Created on February 28, 2019, 7:54 PM
 */

#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;

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();
    DataStore * complementary_data= new DataStore();
    DataStore * pid_data = new DataStore();
    DataStore * FtoW_data = new DataStore();
    Sensor * de_sensor = new Sensor();
    RuwDataFilter * filter = new RuwDataFilter();
    ComplementaryFilter * com_filter = new ComplementaryFilter();
    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)
    filter->m_ruwe_state_data = ruwe_data;     // (2)
    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)
    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 < 2) {
        led = !led;
        filter->FilterIt();
        com_filter->CalculateRealHeight();
        PID->PID_in();
        FtoW->MMA();
        moving->Move();
        de_sensor->get_data();
    N=N+1;    
    }

    // Als de klus klaar is ruim de objecten op.    
    delete (ruwe_data);
    delete (filtered_data);
    delete (pid_data);
    delete (de_sensor);
    delete (filter);
    delete (complementary_data);
    delete (PID);
    delete (com_filter);
    delete (FtoW);
    delete (moving);

    return 0;
}