void Device::analogWritePin(pin_t pin, uint8_t value)
{
if (isPinValid(pin))
analogWrite(pinMap[pin], value);
}
void Motor::stop(void)
{
digitalWrite(direction_pin, FORWARD);
analogWrite(speed_pin, 0);
}
void tembooAnalogWrite(void* sensorConfig, int val) {
TembooGPIOConfig* config = (TembooGPIOConfig*) sensorConfig;
analogWrite(config->pin, val);
config->currentValue = val;
}
void RGBLed::mix(int color1, int color2) {
analogWrite(_pins[color1], 100);
analogWrite(_pins[color2], 100);
}
void Motor::forward(unsigned char speed)
{
digitalWrite(direction_pin, FORWARD);
analogWrite(speed_pin, speed);
}
// Modify the LED colour depending on the current state and button state
void led() {
// If button has been pressed and it is possible to advance to the next state
if (digitalRead(BUTTON) == HIGH &&
STATE != STATE_NO_SD &&
STATE != STATE_SD_SETUP &&
STATE != STATE_DATA_CONCLUDE &&
STATE != STATE_ERROR) {
// Solid white
ledWhite();
}
// If button has not been pressed
else {
switch (STATE) {
case STATE_NO_SD:
// Pulsing red/orange
analogWrite(LED_RED, LED_ON);
analogWrite(LED_GREEN, pulse()/2);
analogWrite(LED_BLUE, LED_OFF);
break;
case STATE_SD_SETUP:
// Pulsing blue/magenta
analogWrite(LED_RED, pulse()/2);
analogWrite(LED_GREEN, LED_OFF);
analogWrite(LED_BLUE, LED_ON);
break;
case STATE_STANDBY:
// Pulsing blue/cyan
analogWrite(LED_RED, LED_OFF);
analogWrite(LED_GREEN, pulse());
analogWrite(LED_BLUE, LED_ON);
break;
case STATE_DATA_SETUP:
// Colour controlled within setup function
break;
case STATE_DATA_COLLECT:
// Solid green
ledGreen();
break;
case STATE_DATA_CONCLUDE:
// Pulsing blue/magenta
analogWrite(LED_RED, pulse()/2);
analogWrite(LED_GREEN, LED_OFF);
analogWrite(LED_BLUE, LED_ON);
break;
case STATE_WARNING:
// Blinking red/yellow
if ((PULSE_COUNT >= LED_ON && PULSE_COUNT <= 63) ||
(PULSE_COUNT >= 128 && PULSE_COUNT <= 191)) ledRed();
else ledYellow();
break;
case STATE_ERROR:
// Blinking red/white
if ((PULSE_COUNT >= LED_ON && PULSE_COUNT <= 63) ||
(PULSE_COUNT >= 128 && PULSE_COUNT <= 191)) ledRed();
else ledWhite();
break;
} // End of switch
} // End of if
// Increment PULSE_COUNT
PULSE_COUNT = (PULSE_COUNT + 1)%256;
} // End of led
// Make RGB LED solid green
void ledGreen() {
analogWrite(LED_RED, LED_OFF);
analogWrite(LED_GREEN, LED_ON);
analogWrite(LED_BLUE, LED_OFF);
} // End of ledGreen
void buzzer::set_value(uint8_t value) {
analogWrite(this->pin, value);
this->value = value;
}
void loop() {
// put your main code here, to run repeatedly:
String str = Serial.readStringUntil('\n');
const char *cmd = str.c_str();
if(strncmp(cmd, "AT+ SetTemp", 11) == 0 && strlen(cmd) > 13) {
const char *target = cmd + 12;
targetTemp = (float)atoi(target) / 100.0f;
// Don't allow 100C to be exceeded.
if(targetTemp > 100.0f) targetTemp = 100.0f;
// Convert to Kelvin
targetTemp += 273.15;
// PID SetMode method ignores if we go from automatic
// to automatic
pid.SetMode(AUTOMATIC);
Serial.print("AT- SetTempOk\r\n");
} else if(strncmp(cmd, "AT+ GetActualTemp", 17) == 0) {
char buf[64];
snprintf(buf, 64, "AT- ActualTemp %u\r\n", (unsigned int)((actualTemp-273.15) * 100));
Serial.print(buf);
} else if(strncmp(cmd, "AT+ GetTargetTemp", 17) == 0) {
char buf[64];
snprintf(buf, 64, "AT- TargetTemp %u\r\n", (unsigned int)((targetTemp-273.15) * 100));
Serial.print(buf);
} else if(strncmp(cmd, "AT+ TurnOff", 11) == 0) {
pid.SetMode(MANUAL);
pulseWidth = 0;
Serial.print("AT- TurnOffOk\r\n");
} else if(str.length() > 0) {
Serial.print("AT- UnknownCmd\r\n");
}
// Get actual temp from thermistor
// Using Steinhart-Hart. Based on
// http://playground.arduino.cc/ComponentLib/Thermistor2
int pinValue = analogRead(pinThermistor);
//float v1 = (float)pinValue / 1024.0f * supplyVoltage;
// Simple voltage divider.
// v1 = supplyVoltage * Rt / (balanceResistor + Rt)
// Solve for Rt.
float rVal = balanceResistor * (1023.0f/pinValue-1);
float lnR = log(rVal);
float tinv = tA + tB * lnR + tC * lnR * lnR * lnR;
actualTemp = 1.0f / tinv;
if(fabs(actualTemp - targetTemp) > 5) {
pid.SetTunings(kPfar, kIfar, kDfar);
} else {
pid.SetTunings(kPnear, kInear, kDnear);
}
pid.Compute();
// TODO: not sure if analogWrite will work correctly,
// may turn heater pad on/off too quickly or relay may be
// too slow.
analogWrite(pinHeater, pulseWidth * 255);
}
void motorClass::stop_all()
{
for (int i = 0 ; i < 4; i++) {
analogWrite(Motor_Enable[i],0);
}
}
/**
* Uses PID control to go toward a block. Tries to keep the block aligned with _center.
*/
void Drivetrain::goToFishPID(Block block, unsigned long currentTime)
{
//Determine PID output
int dt = currentTime - _previousTime; //Find how long has passed since the last adjustment.
_previousTime = currentTime;
Serial.print("dt: ");
Serial.println(dt);
//Determine error; how far off the robot is from center
int error = _center - block.x;
Serial.print("error: ");
Serial.println(error);
//Determine integral; sum of all errors
_integral += error*dt / 1000.0; //Divide by 1000 because dt is milliseconds, adjust for seconds
//Integral is not correct value
Serial.print("integral: ");
Serial.println(_integral);
//Determine derivative; rate of change of errors
float derivative = 1000.0*(error - _previousError) / dt; //Multiply by 1000 because dt is milliseconds, adjust for seconds
Serial.print("derivative: ");
Serial.println(derivative);
//Determine output
int output = (int) (_kp*error + _ki*_integral + _kd*derivative);
Serial.print("adjustment: ");
Serial.println(output);
_previousError = error;
//Go to the fish with the adjusted power values.
//Before adjustment for PWM limits
int rightPower = _power + output;
int leftPower = _power - output;
//After adjustment for PWM limits
if(rightPower < 0)
{
rightPower = 0;
}
else if(rightPower > 255)
{
rightPower = 255;
}
if(leftPower < 0)
{
leftPower = 0;
}
else if(leftPower > 255)
{
leftPower = 255;
}
Serial.print("rightPower: ");
Serial.println(rightPower);
Serial.print("leftPower: ");
Serial.println(leftPower);
//Go with new adjustments
analogWrite(_rightMotorForward, rightPower);
analogWrite(_rightMotorBackward, 0);
analogWrite(_leftMotorForward, leftPower);
analogWrite(_leftMotorBackward, 0);
}
void motorClass::stop(int motorVal)
{
analogWrite(Motor_Enable[motorVal], 0);
}
void ConnectorPulse::addValue(const uint8_t d)
{
this->_value = constrain(this->getValue() + d, 0, (uint8_t) -1);
analogWrite(this->getPin(), this->getValue());
}
void move_stop() {
analogWrite(LEFT_MOTOR_EN, 0);
analogWrite(RIGHT_MOTOR_EN, 0);
}
threeMotorsDriver::threeMotorsDriver()
{
// Pin map
// pins used are 0,1,3,4,5,6,16,17,18,19,22,24,26,30,32,34,40,42
// the analog pins used, A5,A6,A7,A8, are set as #defines in the threeMotorsDriver.h file
// if we later decide to use the servo library, then pins 9 and 10 will not be available for analogWrite()
// available for future use: interrupt pins 2, PWM pins 7 - 13
// available for I2C are pins 20 and 21 (these are also interrupt pins)
IN1A = 24;
IN2A = 22;
STATUSA = 26;
ENABLEAB = 28;
PWMA = 4; // PWM: 0 to 13. Provide 8-bit PWM output with the analogWrite() function.
IN1B = 32;
IN2B = 30;
STATUSB = 34;
PWMB = 5;
IN1C = 40;
IN2C = 42;
STATUSC = 38;
ENABLEC = 36;
PWMC = 6;
// interrupt pins on the mega are:
// 2 (interrupt 0), 3 (interrupt 1), 18 (interrupt 5), 19 (4), 20 (3), and 21 (2)
// we use them for motor encoders
ENCA = 18;
INTERRUPTA = 5;
ENCB = 19;
INTERRUPTB = 4;
ENCC = 20;
INTERRUPTC = 3;
pinMode(IN1A,OUTPUT);
pinMode(IN2A,OUTPUT);
pinMode(STATUSA,INPUT);
pinMode(ENABLEAB,OUTPUT);
pinMode(PWMA,OUTPUT); // PWM: 0 to 13. Provide 8-bit PWM output with the analogWrite() function.
pinMode(IN1B,OUTPUT);
pinMode(IN2B,OUTPUT);
pinMode(STATUSB,INPUT);
pinMode(PWMB,OUTPUT);
pinMode(IN1C,OUTPUT);
pinMode(IN2C,OUTPUT);
pinMode(STATUSC,INPUT);
pinMode(ENABLEC,OUTPUT);
pinMode(PWMC,OUTPUT);
// interrupt pins on the mega are:
// 2 (interrupt 0), 3 (interrupt 1), 18 (interrupt 5), 19 (4), 20 (3), and 21 (2)
// we use them for motor encoders
pinMode(ENCA,INPUT);
pinMode(ENCB, INPUT);
pinMode(ENCC, INPUT);
// turn on pullup resistors
digitalWrite(ENCA, HIGH);
digitalWrite(ENCB, HIGH);
digitalWrite(ENCC, HIGH);
// start disabled, with directions set to forward
digitalWrite(ENABLEAB, LOW);
digitalWrite(IN1A,HIGH);
digitalWrite(IN2A,LOW);
analogWrite(PWMA, 0);
digitalWrite(ENABLEAB, LOW);
digitalWrite(IN1B,HIGH);
digitalWrite(IN2B,LOW);
analogWrite(PWMB, 0);
digitalWrite(ENABLEC, LOW);
digitalWrite(IN1C,HIGH);
digitalWrite(IN2C,LOW);
analogWrite(PWMC, 0);
}
//
// Recursive descent parser, old-school style.
//
void getfactor(void) {
numvar thesymval = symval;
byte thesym = sym;
getsym(); // eat the sym we just saved
switch (thesym) {
case s_nval:
vpush(thesymval);
break;
case s_nvar:
if (sym == s_equals) { // assignment, push is after the break;
getsym();
assignVar(thesymval, getnum());
}
else if (sym == s_incr) { // postincrement nvar++
vpush(getVar(thesymval));
assignVar(thesymval, getVar(thesymval) + 1);
getsym();
break;
}
else if (sym == s_decr) { // postdecrement nvar--
vpush(getVar(thesymval));
assignVar(thesymval, getVar(thesymval) - 1);
getsym();
break;
}
vpush(getVar(thesymval)); // both assignment and reference get pushed here
break;
case s_nfunct:
dofunctioncall(thesymval); // get its value onto the stack
break;
// Script-function-returning-value used as a factor
case s_script_eeprom: // macro returning value
callscriptfunction(SCRIPT_EEPROM, findend(thesymval));
break;
case s_script_progmem:
callscriptfunction(SCRIPT_PROGMEM, thesymval);
break;
case s_script_file:
callscriptfunction(SCRIPT_FILE, (numvar) 0); // name implicitly in idbuf!
break;
case s_apin: // analog pin reference like a0
if (sym == s_equals) { // digitalWrite or analogWrite
getsym();
analogWrite(thesymval, getnum());
vpush(expval);
}
else vpush(analogRead(thesymval));
break;
case s_dpin: // digital pin reference like d1
if (sym == s_equals) { // digitalWrite or analogWrite
getsym();
digitalWrite(thesymval, getnum());
vpush(expval);
}
else vpush(digitalRead(thesymval));
break;
case s_incr:
if (sym != s_nvar) expected(M_var);
assignVar(symval, getVar(symval) + 1);
vpush(getVar(symval));
getsym();
break;
case s_decr: // pre decrement
if (sym != s_nvar) expected(M_var);
assignVar(symval, getVar(symval) - 1);
vpush(getVar(symval));
getsym();
break;
case s_arg: // arg(n) - argument value
if (sym != s_lparen) expectedchar(s_lparen);
getsym(); // eat '('
vpush(getarg(getnum()));
if (sym != s_rparen) expectedchar(s_rparen);
getsym(); // eat ')'
break;
case s_lparen: // expression in parens
getexpression();
if (exptype != s_nval) expected(M_number);
if (sym != s_rparen) missing(M_rparen);
vpush(expval);
getsym(); // eat the )
break;
//
// The Family of Unary Operators, which Bind Most Closely to their Factor
//
case s_add: // unary plus (like +3) is kind of a no-op
getfactor(); // scan a factor and leave its result on the stack
break; // done
case s_sub: // unary minus (like -3)
getfactor();
vpush(-vpop()); // similar to above but we adjust the stack value
break;
case s_bitnot:
getfactor();
vpush(~vpop());
break;
case s_logicalnot:
getfactor();
vpush(!vpop());
break;
case s_bitand: // &var gives address-of-var; ¯o gives eeprom address of macro
if (sym == s_nvar) vpush((numvar) &vars[symval]);
else if (sym == s_script_eeprom) vpush(symval);
else expected(M_var);
getsym(); // eat the var reference
break;
case s_mul: // *foo is contents-of-address-foo; *foo=bar is byte poke assignment
/*****
// what is really acceptable for an lvalue here? ;)
// *y = 5 is failing now by assigning 5 to y before the * is dereferenced
// due to calling getfactor
// everything else works :(
*****/
getfactor();
#if 0
if (sym == s_equals) {
getsym(); // eat '='
getexpression();
* (volatile byte *) vpop() = (byte) expval;
vpush((numvar) (byte) expval);
}
else
#endif
vpush((numvar) (* (volatile byte *) vpop()));
break;
default:
unexpected(M_number);
}
}
// Turn RGB LED off
void ledOff() {
analogWrite(LED_RED, LED_OFF);
analogWrite(LED_GREEN, LED_OFF);
analogWrite(LED_BLUE, LED_OFF);
} // End of ledOff
// Helper function for simply sending an RGB signal out to indicate a debug or testing condition.
void rgbDebug(byte r, byte g, byte b)
{
analogWrite(9, r);
analogWrite(6, g);
analogWrite(5, b);
} // rgbDebug()
// Make RGB LED solid white
void ledWhite() {
analogWrite(LED_RED, LED_ON);
analogWrite(LED_GREEN, LED_ON);
analogWrite(LED_BLUE, LED_ON);
} // End of ledWhite
void check_axes_activity() {
unsigned char axis_active[NUM_AXIS] = { 0 },
tail_fan_speed = fanSpeed;
#ifdef BARICUDA
unsigned char tail_valve_pressure = ValvePressure,
tail_e_to_p_pressure = EtoPPressure;
#endif
block_t *block;
if (blocks_queued()) {
uint8_t block_index = block_buffer_tail;
tail_fan_speed = block_buffer[block_index].fan_speed;
#ifdef BARICUDA
block = &block_buffer[block_index];
tail_valve_pressure = block->valve_pressure;
tail_e_to_p_pressure = block->e_to_p_pressure;
#endif
while (block_index != block_buffer_head) {
block = &block_buffer[block_index];
for (int i=0; i<NUM_AXIS; i++) if (block->steps[i]) axis_active[i]++;
block_index = next_block_index(block_index);
}
}
if (DISABLE_X && !axis_active[X_AXIS]) disable_x();
if (DISABLE_Y && !axis_active[Y_AXIS]) disable_y();
if (DISABLE_Z && !axis_active[Z_AXIS]) disable_z();
if (DISABLE_E && !axis_active[E_AXIS]) {
disable_e0();
disable_e1();
disable_e2();
disable_e3();
}
#if HAS_FAN
#ifdef FAN_KICKSTART_TIME
static millis_t fan_kick_end;
if (tail_fan_speed) {
millis_t ms = millis();
if (fan_kick_end == 0) {
// Just starting up fan - run at full power.
fan_kick_end = ms + FAN_KICKSTART_TIME;
tail_fan_speed = 255;
} else if (fan_kick_end > ms)
// Fan still spinning up.
tail_fan_speed = 255;
} else {
fan_kick_end = 0;
}
#endif //FAN_KICKSTART_TIME
#ifdef FAN_MIN_PWM
#define CALC_FAN_SPEED (tail_fan_speed ? ( FAN_MIN_PWM + (tail_fan_speed * (255 - FAN_MIN_PWM)) / 255 ) : 0)
#else
#define CALC_FAN_SPEED tail_fan_speed
#endif // FAN_MIN_PWM
#ifdef FAN_SOFT_PWM
fanSpeedSoftPwm = CALC_FAN_SPEED;
#else
analogWrite(FAN_PIN, CALC_FAN_SPEED);
#endif // FAN_SOFT_PWM
#endif // HAS_FAN
#ifdef AUTOTEMP
getHighESpeed();
#endif
#ifdef BARICUDA
#if HAS_HEATER_1
analogWrite(HEATER_1_PIN,tail_valve_pressure);
#endif
#if HAS_HEATER_2
analogWrite(HEATER_2_PIN,tail_e_to_p_pressure);
#endif
#endif
}
// Make RGB LED solid magenta
void ledMagenta() {
analogWrite(LED_RED, LED_ON);
analogWrite(LED_GREEN, LED_OFF);
analogWrite(LED_BLUE, LED_ON);
} // End of ledMagenta
void Alarm::toggleAlarm()
{
analogWrite(this->inPin, this->currentState*200);
}
void Motores::parar() {
noInterrupts();
analogWrite(m1_pwm,0);
analogWrite(m2_pwm,0);
interrupts();
}
void CheapieBlade::PerformIO()
{
analogWrite(mLedPin, mLedPowerLevel);
}
void Motor::reverse(unsigned char speed)
{
digitalWrite(direction_pin, REVERSE);
analogWrite(speed_pin, speed);
}
void threeMotorsDriver::setBrakesAB()
{
digitalWrite(ENABLEAB, LOW);
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
}
void check_axes_activity()
{
unsigned char x_active = 0;
unsigned char y_active = 0;
unsigned char z_active = 0;
unsigned char j_active = 0;
unsigned char e_active = 0;
unsigned char tail_fan_speed = fanSpeed;
#ifdef BARICUDA
unsigned char tail_valve_pressure = ValvePressure;
unsigned char tail_e_to_p_pressure = EtoPPressure;
#endif
block_t *block;
if(block_buffer_tail != block_buffer_head)
{
uint8_t block_index = block_buffer_tail;
tail_fan_speed = block_buffer[block_index].fan_speed;
#ifdef BARICUDA
tail_valve_pressure = block_buffer[block_index].valve_pressure;
tail_e_to_p_pressure = block_buffer[block_index].e_to_p_pressure;
#endif
while(block_index != block_buffer_head)
{
block = &block_buffer[block_index];
if(block->steps_x != 0) x_active++;
if(block->steps_y != 0) y_active++;
if(block->steps_z != 0) z_active++;
if(block->steps_j != 0) j_active++;
if(block->steps_e != 0) e_active++;
block_index = (block_index+1) & (BLOCK_BUFFER_SIZE - 1);
}
}
if((DISABLE_X) && (x_active == 0)) disable_x();
if((DISABLE_Y) && (y_active == 0)) disable_y();
if((DISABLE_Z) && (z_active == 0)) disable_z();
if((DISABLE_J) && (j_active == 0)) disable_j();
if((DISABLE_E) && (e_active == 0))
{
disable_e0();
disable_e1();
disable_e2();
}
#if defined(FAN_PIN) && FAN_PIN > -1
#ifdef FAN_KICKSTART_TIME
static unsigned long fan_kick_end;
if (tail_fan_speed) {
if (fan_kick_end == 0) {
// Just starting up fan - run at full power.
fan_kick_end = millis() + FAN_KICKSTART_TIME;
tail_fan_speed = 255;
} else if (fan_kick_end > millis())
// Fan still spinning up.
tail_fan_speed = 255;
} else {
fan_kick_end = 0;
}
#endif//FAN_KICKSTART_TIME
#ifdef FAN_SOFT_PWM
fanSpeedSoftPwm = tail_fan_speed;
#else
analogWrite(FAN_PIN,tail_fan_speed);
#endif//!FAN_SOFT_PWM
#endif//FAN_PIN > -1
#ifdef AUTOTEMP
getHighESpeed();
#endif
#ifdef BARICUDA
#if defined(HEATER_1_PIN) && HEATER_1_PIN > -1
analogWrite(HEATER_1_PIN,tail_valve_pressure);
#endif
#if defined(HEATER_2_PIN) && HEATER_2_PIN > -1
analogWrite(HEATER_2_PIN,tail_e_to_p_pressure);
#endif
#endif
}
void threeMotorsDriver::setBrakesC()
{
digitalWrite(ENABLEC, LOW);
analogWrite(PWMC, 0);
}
// A thread which handle car's actions without interfear main thread.
void carTask(const void *arg) {
Car2wd *pCar = (Car2wd *) arg;
os_event_t evt;
uint32_t dfactor;
uint32_t dport;
uint32_t dbitmask;
while(1) {
// wait signal for infinite timeout
evt = os_signal_wait(0, 0xFFFFFFFF);
if (evt.status != OS_EVENT_SIGNAL) {
continue;
}
if (evt.value.signals & CAR_SPEED_CHANGE) {
analogWrite(pCar->enA, pCar->currentSpeed);
analogWrite(pCar->enB, pCar->currentSpeed);
/* only apply to forward right/left and backward right/left
* The motor run time is 20ms, and stop time is 20ms~360ms.
* speed - min_speed
* dfactor = 20 + 360 * -------------------
* 255 - min_speed
*/
dfactor = 20 + (360 * (pCar->currentSpeed - CAR_MIN_SPEED))/(255 - CAR_MIN_SPEED);
}
if (evt.value.signals & CAR_ACTION_CHANGE) {
switch(pCar->currentAction) {
case CAR_STOP:
*portOutputRegister(pCar->in1_port) &= ~(pCar->in1_bitmask);
*portOutputRegister(pCar->in2_port) &= ~(pCar->in2_bitmask);
*portOutputRegister(pCar->in3_port) &= ~(pCar->in3_bitmask);
*portOutputRegister(pCar->in4_port) &= ~(pCar->in4_bitmask);
break;
case CAR_FORWARD:
case CAR_FORWARD_RIGHT:
case CAR_FORWARD_LEFT:
*portOutputRegister(pCar->in1_port) |= (pCar->in1_bitmask);
*portOutputRegister(pCar->in2_port) &= ~(pCar->in2_bitmask);
*portOutputRegister(pCar->in3_port) |= (pCar->in3_bitmask);
*portOutputRegister(pCar->in4_port) &= ~(pCar->in4_bitmask);
if (pCar->currentAction == CAR_FORWARD_RIGHT) {
dport = pCar->in3_port;
dbitmask = pCar->in3_bitmask;
}
if (pCar->currentAction == CAR_FORWARD_LEFT) {
dport = pCar->in1_port;
dbitmask = pCar->in1_bitmask;
}
break;
case CAR_BACKWARD:
case CAR_BACKWARD_RIGHT:
case CAR_BACKWARD_LEFT:
*portOutputRegister(pCar->in1_port) &= ~(pCar->in1_bitmask);
*portOutputRegister(pCar->in2_port) |= (pCar->in2_bitmask);
*portOutputRegister(pCar->in3_port) &= ~(pCar->in3_bitmask);
*portOutputRegister(pCar->in4_port) |= (pCar->in4_bitmask);
if (pCar->currentAction == CAR_BACKWARD_RIGHT) {
dport = pCar->in4_port;
dbitmask = pCar->in4_bitmask;
}
if (pCar->currentAction == CAR_BACKWARD_LEFT) {
dport = pCar->in2_port;
dbitmask = pCar->in2_bitmask;
}
break;
case CAR_ROTATE_RIGHT:
*portOutputRegister(pCar->in1_port) |= (pCar->in1_bitmask);
*portOutputRegister(pCar->in2_port) &= ~(pCar->in2_bitmask);
*portOutputRegister(pCar->in3_port) &= ~(pCar->in3_bitmask);
*portOutputRegister(pCar->in4_port) |= (pCar->in4_bitmask);
break;
case CAR_ROTATE_LEFT:
*portOutputRegister(pCar->in1_port) &= ~(pCar->in1_bitmask);
*portOutputRegister(pCar->in2_port) |= (pCar->in2_bitmask);
*portOutputRegister(pCar->in3_port) |= (pCar->in3_bitmask);
*portOutputRegister(pCar->in4_port) &= ~(pCar->in4_bitmask);
break;
}
}
if (pCar->currentAction >= CAR_FORWARD_RIGHT && pCar->currentAction <= CAR_BACKWARD_LEFT) {
#if DELAY_TURN
while(1) {
*portOutputRegister(dport) |= dbitmask;
evt = os_signal_wait(0, 20);
if (evt.status == OS_EVENT_SIGNAL) {
os_signal_set(pCar->tid, evt.value.signals);
break;
}
*portOutputRegister(dport) &= ~dbitmask;
evt = os_signal_wait(0, dfactor);
if (evt.status == OS_EVENT_SIGNAL) {
*portOutputRegister(dport) |= dbitmask;
os_signal_set(pCar->tid, evt.value.signals);
break;
}
}
#else
*portOutputRegister(dport) &= ~dbitmask;
#endif
}
}
}
void DCMotor::Stop()
{
analogWrite(M_PWMPin, LOWER_ANALOG_LIMIT);
}
