3-Phase-Controller/Interpreter.cpp at master · rickyfajar93/3-Phase-Controller · 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

#include "Interpreter.h"
#include "Config.h"
#include "ThreePhaseController.h"
#include "ThreePhaseDriver.h"
#include "Predictor.h"
// #include "MotorControl.h"
#include "MLX90363.h"
#include "TwillBotInterface.h"

bool Interpreter::streaming = true;

void Interpreter::interpretFromMaster(u1 const * const incomingData){
	if (checkCRC(incomingData)){
		return;
	}

	if (incomingData[0] == 0x20){
		s2 torque = *((s2*)(incomingData+1));

		if(torque > ThreePhaseController::getMaxTorque())
			torque = ThreePhaseController::getMaxTorque();
		else if ( torque < -ThreePhaseController::getMaxTorque())
			torque = -ThreePhaseController::getMaxTorque();

		ThreePhaseController::setTorque(torque);
	}

	if (incomingData[0] == 0x10) {
		s4 go = *((s4*)(incomingData+1));


	}

	if (incomingData[0] == 0x88){
		//Start Streaming
		if (incomingData[1] == 0xF0){
			streaming = true;
		}
		//Stop Streaming
		else if (incomingData[1] == 0x0F){
			streaming = false;
		}
	}

	//deadtime configuration
	if (incomingData[0] == 0x40){
		if (incomingData[1] == 0xF0){
			//advance to next DeadTime
			ThreePhaseController::setDeadTimes(ThreePhaseController::getDeadTimes()+0x11);

		}
		else if (incomingData[1] == 0x0F){
			//decement to last DeadTime
			ThreePhaseController::setDeadTimes(ThreePhaseController::getDeadTimes()-0x11);

		}
		else if (incomingData[1] == 0xFF){
			ThreePhaseController::setDeadTimes(incomingData[2]);
		}
	}

	if (incomingData[0] == 0x41){
		ThreePhaseDriver::setAmplitude(incomingData[1]);
	}

	if (incomingData[0] == 0x42){
		ThreePhaseDriver::advanceTo((((u2)incomingData[2]) << 8) | incomingData[1]);
	}

	if (incomingData[0] == 0x43){
		Predictor::setAdjustVal(incomingData[1]);
	}

	if (incomingData[0] == 0x44){
		Predictor::setPhaseAdvanceRatio(incomingData[1]);
	}
}

//HACK ME
extern u1 resetCause;

void Interpreter::sendNormalDataToMaster(){
	// if (!streaming) return;
	// if (!MLX90363::isMeasurementReady()) return;

	static u2 extra = 0;

	u1 * const buff = TwillBotInterface::getOutgoingWriteBuffer();

	*(u2 * const)(&buff[0]) = ThreePhaseController::getRoll();
	*(u2 * const)(&buff[2]) = ThreePhaseController::getVelocity();
	*(u2 * const)(&buff[4]) = ThreePhaseController::getMeasuredPosition();
	buff[6] = Predictor::getAdjustVal();
	buff[7] = Predictor::getPhaseAdvanceRatio();
	buff[8] = (u1)ThreePhaseController::getTorque();
	buff[9] = (u1)ThreePhaseController::getDeadTimes();

	TwillBotInterface::getOutgoingWriteBuffer()[Config::i2cBufferOutgoingSize-1] = getCRC(TwillBotInterface::getOutgoingWriteBuffer(), Config::i2cBufferOutgoingSize-1);

	TwillBotInterface::releaseNextWriteBuffer();
}

//check the message to check if it passes CRC check
bool Interpreter::checkCRC(u1 const * const mes){
	u1 sum = 0;

	for(u1 i = 0; i < Config::i2cBufferIncomingSize; i++)
		//sum = _crc8_ccitt_update(sum, mes[i]);
		sum += mes[i];

	return sum != 0xFF;
}

u1 Interpreter::getCRC(u1 const * const mes, u1 length){
	u1 sum = 0;

	for(u1 i = 0; i < length; i++)
		//sum = _crc8_ccitt_update(sum, mes[i]);
		sum += mes[i];

	return ~sum;
}