// the loop routine runs over and over again forever:
void loop() {
// TODO add print status
sampleSensors();
speedSteeringControlMap();
adjustForLeanMode();
steerControl();
leanControl();
brakeControl();
hydraulicControl();
tractionMotorCommandProcessing();
setThrottle();
setRevValues();
//analog inputs
joystickValy = readAnalog(joystickFBSensor);
joystickValx = readAnalog(joystickLRSensor);
int PumpVal = readAnalog(A2);
int BrakeSensor = readAnalog(A3);
//PWM inputs
int SteerSensor = readAnalog(A4);
int LeanSensor = readAnalog(A4);
int SpeedSensor = readAnalog(A4);
PWMOutput2(joystickValx, SteerSensor, 10);
button_test();
led_test();
digitalWrite(pSpeedSenseIn, HIGH);
delay(30); // delay in between reads for stability
}
int main(void)
{
int LogOn = 0;
int ComOn = 0;
int dT;
int16_t data[32];
uint32_t x;
UINT cnt;
InitSystemTick();
InitGPIO();
InitTimer();
STM_EVAL_LEDOn(LED4);
InitUART(115200);
InitPressureSensor();
InitFlowMeter();
InitAccAndMag();
InitGyro();
STM_EVAL_LEDOn(LED7); // zapnem LED7 - zelena
Delta_us();
while(1)
{
// sample period is time elapsed since previous sampling of sensors
sampleSensors(a,m,g);
// update timebase for next time
dT = Delta_us();
samplePeriod = 0.000001f * dT;
// convert gyro deg/s to rad/s
imuDegToRadV3(g);
// update AHRS
MadgwickFullAHRSUpdate(g, a, m, samplePeriod, quaternion);
if (LogOn) {
sprintf(text, "%5d%6d%6d%6d%7d%7d%7d%5d%5d%5d\r\n",
dT,
aRawData[0], aRawData[1], aRawData[2],
gRawData[0], gRawData[1], gRawData[2],
mRawData[0], mRawData[1], mRawData[2]);
f_write(&File, text, strlen(text), &cnt);
}
if(STM_EVAL_PBGetState(BUTTON_USER)) { // zmena rezimu vystupu
if (LogOn) {
// Stop logdata
LogOn = 0;
STM_EVAL_LEDOff(LED5); // zapnem LED7 - zelena
if (f_close(&File) == 0)
xprintf("Stop login data.\n\r");
else
xprintf("Error write datafile.\n\r");
} else {
// Start logdata
if (newFile() == 0)
xprintf("Start login data.\n\r");
else
xprintf("Error create datafile.\n\r");
STM_EVAL_LEDOn(LED5); // zapnem LED7 - zelena
LogOn = 1;
}
}
if (ComOn) {
data[0] = dT;
data[1] = aRawData[0];
data[2] = aRawData[1];
data[3] = aRawData[2];
data[4] = gRawData[0];
data[5] = gRawData[1];
data[6] = gRawData[2];
data[7] = mRawData[0];
data[8] = mRawData[1];
data[9] = mRawData[2];
data[10] = 0x8080;
SendDataUART((uint8_t*) data, 22);
}
if (IsReceiveUART()) {
if (ReadByteUART() == 's') {
STM_EVAL_LEDOn(LED10); // zapnem LED7 - zelena
ComOn = 1;
} else {
STM_EVAL_LEDOff(LED10); // zapnem LED7 - zelena
ComOn = 0;
}
}
/*
if (x = GetFlowMeter()) {
printf("Flow: %d\n\r", x);
}
*/
}
}
