OpenFFBoard/Firmware/FFBoard/Src/AxesManager.cpp at 5b22dade0089e7fca16dfe1a67c81a3f57d80f8f · Ultrawipf/OpenFFBoard · 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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
/*
 * AxesManager.cpp
 *
 *  Created on: 30.01.21
 *      Author: Jon Lidgard
 */

#include <stdint.h>
#include <string>
#include "constants.h"
#include "cmsis_os.h"
#include "AxesManager.h"


//ClassIdentifier AxesManager::info = {
//		.name 	= "NONE" ,
//			 .id	= 0,
//			 .hidden = true
//};
//const ClassIdentifier AxesManager::getInfo(){
//	return info;
//}

AxesManager::AxesManager(volatile Control_t* control) {
	this->control = control;
	//this->restoreFlash();
}

AxesManager::~AxesManager() {
	deleteAxes();
}

void AxesManager::deleteAxes(){
//	for (auto axis : axes) {
//		delete axis;
//	}
	axes.clear();
	axis_count = 0;
}

void AxesManager::setEffectsCalculator(EffectsCalculator* calc) {
	this->effects_calc = calc;
}

//void AxesManager::restoreFlash() {
//	uint16_t val;
//	bool res = (Flash_Read(ADR_AXIS_COUNT, &val));
//
//	if (!res || !this->validAxisRange(val)) {
//		val = 1;
//	}
//	this->setAxisCount(val);
////	for (auto &axis : axes) {
////		axis->restoreFlash();
////	}
//}
//
//void AxesManager::saveFlash() {
//	Flash_Write(ADR_AXIS_COUNT, this->axis_count);
//}

void AxesManager::update() {
	for (auto &axis: axes) {
		axis->prepareForUpdate();
	}

	effects_calc->calculateEffects(axes);
}

void AxesManager::updateTorque() {
	for (auto &axis: axes) {
		axis->updateDriveTorque();
	}
}

void AxesManager::cfEffectsFreqToEffectsCalc(uint16_t freq){
	effects_calc->setCfEffectsFreq(freq);
}

std::vector<int32_t>* AxesManager::getAxisValues(){
	this->axisValues.clear(); // Empty axes
	for (std::size_t i=0; i < axes.size(); i++) {
		Encoder* enc = axes[i]->getEncoder();
		if(enc && enc->getEncoderType() != EncoderType::NONE){ // If encoder not none type
			this->axisValues.push_back(axes[i]->getLastScaledEnc());
			//this->axisValues[i] = axes[i]->getLastScaledEnc();
		}
	}
	return &this->axisValues;
}

void AxesManager::emergencyStop(bool reset) {
	for (auto &axis : axes) {
		axis->emergencyStop(reset);
	}
}

uint8_t AxesManager::getAxisCount() { return axis_count; }

bool AxesManager::validAxisRange(uint8_t val) {
	return ((val>0) && (val<=MAX_AXIS));
}

bool AxesManager::setAxisCount(int8_t count) {
	if (!this->validAxisRange(count)) {
		return false; // invalid number of axis
	}
	// Really need to use some form of mutex
	Flash_Write(ADR_AXIS_COUNT, count);

	while (count < axis_count) {
		uint8_t pos = axis_count-1;
		axes.erase(axes.begin()+pos);
		axis_count--;
	}
	while (count > axis_count) {
		axes.push_back( std::make_unique<Axis>('X'+axis_count, control) ); // Axis are indexed from 1-X to 3-Z
		axis_count++;
	}
	control->update_disabled = false;

	// Allocate axis value buffer
	this->axisValues.reserve(axis_count);
	//this->axisValues = std::vector<int32_t>(axis_count,0);

	return true;
}

void AxesManager::usbSuspend() {
		for (auto &axis : axes) {
			axis->usbSuspend();
		}
}

void AxesManager::usbResume() {
		for (auto &axis : axes) {
			axis->usbResume();
		}
}

void AxesManager::resetPosZero() {
		for (auto &axis : axes) {
			axis->setPos(0);
		}
}