void setPWMs(void)
{
#ifndef PID_
setSpeed1((int)(getPIDOut(&PIDPd) * MAXDC));
setSpeed2((int)(getPIDOut(&PIDPg) * MAXDC));
#else
setSpeed1((int)(getPID_Out(&PIDPd) * MAXDC));
setSpeed2((int)(getPID_Out(&PIDPg) * MAXDC));
#endif
}
void setMD49commands(void) {
//set acceleration if command changed since last set of acceleration
if (i2cdata[2] NOT currentAcceleration) {
setAcceleration(i2cdata[2]);
}
//set new mode only if changed
if (i2cdata[3] NOT currentMode) {
setMode(i2cdata[3]);
}
//set speed continuously, when timeout is enabled
if (statusTimeout == 1) {
setSpeed1(i2cdata[0]);
setSpeed2(i2cdata[1]);
}
//set speed once changed, when timeout is disabled
if (statusTimeout == 0) {
if (recentSpeed1 != i2cdata[0]) {
setSpeed1(i2cdata[0]);
}
if (recentSpeed2 != i2cdata[1]) {
setSpeed2(i2cdata[1]);
}
}
//reset encoders if byte was checked
if (i2cdata[4] == 1) {
resetEncoders();
i2cdata[4] = 0;
}
//set regulator if changed
if (i2cdata[5] == 0) {
if (statusRegulator NOT i2cdata[5]) {
disableRegulator();
}
} else if (i2cdata[5] == 1) {
if (statusRegulator NOT i2cdata[5]) {
enableRegulator();
}
}
// set timeout if changed
if (i2cdata[6] == 0) {
if (statusTimeout NOT i2cdata[6]) {
disableTimeout();
}
} else if (i2cdata[6] == 1) {
if (statusTimeout NOT i2cdata[6]) {
enableTimeout();
}
}
}
void setPWMs(void) {
float vg, vd;
#ifndef PID_
transformVMots(getPIDOut(&PIDl), getPIDOut(&PIDt) * ENTRAXE / 2, &vg, &vd);
#else
transformVMots(getPID_Out(&PIDl), getPID_Out(&PIDt) * ENTRAXE / 2, &vg, &vd);
#endif
setSpeed1((int)(vd * MAXDC));
setSpeed2((int)(vg * MAXDC));
}
int main(int argc, char* argv[]) {
DDRC = 0x0f;
DDRD = 0xf0;
DDRB = 0xC3;
PORTB = 0;
PORTC = 0;
PORTD = 0;
serial_init();
send_string_serial("Starting up...\n");
_delay_ms(500);
speed1 = 4000;
speed2 = 4000;
speed3 = 4000;
speed4 = 1250;
counter1 = 1;
counter2 = 1;
counter3 = 1;
counter4 = 1;
needState1 = FALSE;
needState2 = FALSE;
needState3 = FALSE;
movingForward1 = FALSE;
movingForward2 = FALSE;
movingForward3 = FALSE;
currentState1 = 0;
currentState2 = 0;
currentState3 = 0;
motorState1 = getMotorState(currentState1);
motorState2 = getMotorState(currentState2) << 4;
unsigned char state = getMotorState(currentState3);
motorState3 = ((state & 0x0C) << 4) | (state & 0x03);
setupTimers();
char index = 0;
int speeds[3];
char* buffer = (char*) speeds;
while (1) {
if (UCSRA & (1<<RXC)) {
buffer[index++] = UDR;
if (index == 6) {
// do something with the buffer
setSpeed1(speeds[0]);
setSpeed2(speeds[1]);
setSpeed3(speeds[2]);
index = 0;
send_string_serial("Received full message\n\r");
}
}
checkSerialSend();
if (needState1) {
currentState1 = getNextState(currentState1, movingForward1);
motorState1 = getMotorState(currentState1);
needState1 = FALSE;
}
if (needState2) {
currentState2 = getNextState(currentState2, movingForward2);
motorState2 = getMotorState(currentState2) << 4;
needState2 = FALSE;
}
if (needState3) {
currentState3 = getNextState(currentState3, movingForward3);
state = getMotorState(currentState3);
motorState3 = ((state & 0x0C) << 4) | (state & 0x03);
needState3 = FALSE;
}
/*
if (needsNewSpeed) {
currentSpeedIndex = (currentSpeedIndex + 1);
if (currentSpeedIndex == 0x40) {
currentSpeedIndex = 0;
send_string_serial("Loop...\n");
}
setSpeed1(speed[0][currentSpeedIndex]);
setSpeed2(speed[1][currentSpeedIndex]);
setSpeed3(speed[2][currentSpeedIndex]);
needsNewSpeed = FALSE;
}
*/
}
return 0;
}
/*********************************************************************************************************
** Function name: setSpeedDir1
** Descriptions: set motor1 speed and direction
*********************************************************************************************************/
void motor_4wd::setSpeedDir1(int ispeed, unsigned char dir)
{
setSpeed1(ispeed);
setDir1(dir);
setRun1();
}
