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: setSpeedDir2 ** Descriptions: set motor2 speed and direction *********************************************************************************************************/ void motor_4wd::setSpeedDir2(int ispeed, unsigned char dir) { setSpeed2(ispeed); setDir2(dir); setRun2(); }