CANMotor/CANMotorManager.cpp at main · mechaken/CANMotor · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
/*
*This software includes the work that is distributed in the Apache License 2.0
*/

#include "CANMotorManager.h"

#include "CANMotor.h"
#include "mbed.h"
#include <vector>
#include "platform/SingletonPtr.h"
#include "platform/PlatformMutex.h"

std::vector<CANMotor*> CANMotorManager::_motor_ptr;
SingletonPtr<PlatformMutex> CANMotorManager::_mutex;

CANMotorManager::CANMotorManager(CAN &can)
    : _can(can)
{
//    _can.attach(callback(this, &CANMotorManager::parse));
}

CANMotorManager::~CANMotorManager()
{

}

void CANMotorManager::add(CANMotor *ptr)
{
    lock();
    _motor_ptr.push_back(ptr);
    unlock();
}

void CANMotorManager::erase(CANMotor *ptr)
{
    lock();
    for(std::vector<CANMotor*>::iterator itr = _motor_ptr.begin(); itr != _motor_ptr.end(); ++itr)
    {
        if (*itr == ptr)
        {
            _motor_ptr.erase(itr);
            break;
        }
    }
    unlock();
}

int CANMotorManager::connect_all(int interval_ms)
{
    return 0;
//    int miss = 0;
//    int interval_us = interval_ms * 1000;
//
//    for(std::vector<CANMotor*>::iterator itr = _motor_ptr.begin(); itr != _motor_ptr.end(); ++itr)
//    {
//        int timeout = 0;
//        while(((*itr)->connect() == false))
//        {
//            wait_us(interval_us);
//
//            if (timeout++ >= 3)
//            {
//                    miss++;
//                    break;
//            }
//        }
//    }
//
//    return miss;
}

int CANMotorManager::write_all(int interval_ms)
{
    int miss = 0;
    int interval_us = interval_ms * 1000;

    for(std::vector<CANMotor*>::iterator itr = _motor_ptr.begin(); itr != _motor_ptr.end(); ++itr)
    {
        if ((*itr)->write() == 0)
        {
            miss++;
            // debug("Couldn't write to can bus.\r");
            // debug("%d: pwm %0.2f, state %d\r", motor_number, duty_cycle, state);
        }
        wait_us(interval_us);
    }

    return miss;
}

void CANMotorManager::parse()
{
//    _can.read(_msg);
//
//    for(std::vector<CANMotor*>::iterator itr = _motor_ptr.begin(); itr != _motor_ptr.end(); ++itr)
//    {
//        if ((*itr)->id() == _msg.id)
//        {
//            (*itr)->parse(_msg.data);
//            break;
//        }
//    }
}

void CANMotorManager::lock()
{
    _mutex->lock();
}

void CANMotorManager::unlock()
{
    _mutex->unlock();
}