void main()
{
//Iniitalize pulsewidth
FanPWM_WritePulseWidth(255);
//Start Fan PWM
FanPWM_Start();
//Enable FanPWM interrupt
FanPWM_EnableInt();
//Enable global interrupts
M8C_EnableGInt;
//Start tach timer
TachTimer_Start();
//Enable Tach Timer interrupts
TachTimer_EnableInt();
//Start LCD
LCD_Start();
//Enable GPIO interrupts
INT_MSK0 |= 0b00100000;
//Initialize display
LCD_Init();
LCD_Position(0,0);
LCD_PrCString("Pulse Width: ");
LCD_Position(0,13);
LCD_PrHexByte(FanPWM_bReadPulseWidth());
while(1)//control loop
{
if(bDataAvailable == 1)//if data is available
{
//Clear it
bDataAvailable = 0;
//Calculate fan speed and write RPM and num cycles to lCD
wSpeedRPM = ( ( (60/4) * 3000000 * cNumCycles )+ ((wFirstValue - wLastValue)/2) )/(wFirstValue - wLastValue);
//Write to LCD
LCD_Init();
LCD_Position(0,0);
LCD_PrCString("RPM: ");//Begin writing at 0,5
LCD_Position(0,5);
LCD_PrHexInt(wSpeedRPM);
LCD_Position(1,0);
LCD_PrCString("#Cycles: ");//Begin writing at 1,9
LCD_Position(1,9);
LCD_PrHexByte(cNumCycles);
}
}//end control loop
}
void ButtonDown()
{
//Debounce by reading three times
if( (PRT1DR & 0b00000001) == 0b00000001) //1
{
if( (PRT1DR & 0b00000001) == 0b00000001) //2
{
if( (PRT1DR & 0b00000001) == 0b00000001) //3 Button is really down
{
//LCD_Position(0,0);
//LCD_PrCString("Down... ");
//Wait for release
while( (PRT1DR & 0b00000001) == 0b00000001){}
//Record the state of P0[0] by shifting it into the sequence
//Shift the sequence by one bit
sequence <<= 1;
//Read in the current state P0[0]
if( (PRT0DR & 0b00000001) == 0b00000001)
{
//Bit entered was 1
sequence |= 0b00000001;
LCD_Position(1,0);
LCD_PrCString("1");
}
else
{
//Bit entered was 0
sequence &= ~0b00000001;
LCD_Position(1,0);
LCD_PrCString("0");
}
}
}
}
//If sequnce is correct
if( (sequence & 0b00001111) == 0x0D)
{
//Let user known
LCD_Position(0,0);
LCD_PrCString("Correct! ");
//LCD_Position(1,0);
//LCD_PrHexByte(sequence & 0b00001111);
}
else
{
LCD_Position(0,0);
LCD_PrCString("Incorrect! ");
//LCD_Position(1,0);
//LCD_PrHexByte(sequence & 0b00001111);
}
}
void ButtonDown()
{
//Not needed
//Wait for sleep timer
//while(INT_CLR0 & 0b01000000 == 0){}//Wait for posted sleep timer interrupt
//Clear sleep timer interrupt
//INT_CLR0 &= ~0b01000000;
//Read switch input 3 times (debounce)
if(PRT1DR & 0b00000001 == 0b00000001) //First read
{
if(PRT1DR & 0b00000001 == 0b00000001) //Second read
{
if(PRT1DR & 0b00000001 == 0b00000001) //Third read - Button has been pressed
{
while(PRT1DR & 0b00000001 == 0b00000001){}//Wait for button to be released
//Decrement pulse width
FanPWM_WritePulseWidth(FanPWM_bReadPulseWidth() - 1);
//Update display
LCD_Init();
LCD_Position(0,0);
LCD_PrCString("Pulse Width: ");
LCD_Position(0,13);
LCD_PrHexByte(FanPWM_bReadPulseWidth());
}
}
}
}
void main(void)
{
int result;
float voltage;
int status;
M8C_EnableGInt ; // Uncomment this line to enable Global Interrupts
// Insert your main routine code here.
//Start PGA in high power mode
PGA_Start(PGA_HIGHPOWER);
//Start ADCINC in high power mode
ADCINC_Start(ADCINC_HIGHPOWER);
//Start LCD
LCD_Start();
//Run the ADC continuously
ADCINC_GetSamples(0);
SleepTimer_Start();
SleepTimer_SetInterval(SleepTimer_1_HZ);
SleepTimer_EnableInt();
while (1)
{
SleepTimer_SyncWait(1, SleepTimer_WAIT_RELOAD);
// Wait for data to be ready
while (ADCINC_fIsDataAvailable() == 0);
// Get Data and clear flag
result=ADCINC_iClearFlagGetData();
voltage = result * SCALE_FACTOR;
LCD_Position(0, 0);
LCD_PrCString(" ");
LCD_Position(0, 0);
LCD_PrHexInt(result);
LCD_Position(1, 0);
LCD_PrCString(" ");
LCD_Position(1, 0);
LCD_PrString(ftoa(voltage, &status));
}
}
void updateLCD()
{
//Clear LCD
LCD_Init();
//Print month
if(blinkState == 0 && state == 2)
{
//Do not print
}
else
{
//Print month
LCD_Position(0,0);
LCD_PrHexByte(month);
}
LCD_Position(0,2);
LCD_PrCString("/");
//Print day
if(blinkState == 0 && state == 3)
{
//Do not print
}
else
{
//Print day
LCD_Position(0,3);
LCD_PrHexByte(day);
}
LCD_Position(0,5);
LCD_PrCString("/");
//Print year
if(blinkState == 0 && state == 4)
{
//Do not print
}
else
{
//Print year
LCD_Position(0,6);
LCD_PrHexByte(year);
}
}
void main(void)
{
M8C_EnableGInt;
LCD_Start(); // Start LCD
LCD_Position(0,0);
LCD_PrCString("PSoC I2C Slave");
EzI2Cs_SetRamBuffer(1, 1, (char *)&Wert); // Start I²C Buffer, Size of 1 Byte, Allowing to Read/Write 1 Byte
I2C_Init();
while(1)
{
LCD_Position(1,0);
LCD_PrCString("Wert:");
LCD_Position(2,0);
LCD_PrHexInt(Wert);
}
}
// normalization function for Elevator(Timer2)
float EvaluateElevator(DWORD value)
{
// Check if pulsewidth data is available
if(FlagsElevator & DATA_AVAILABLE_ELEVATOR)
{
#if (DEBUG_LCD)
LCD_Position(1,0);
LCD_PrHexInt(value);
#endif
// stick in center
if (Within(value, CENTER_ELEVATOR, MARGIN_ELEVATOR))
{
#if (DEBUG_LCD)
LCD_Position(1,5);
LCD_PrCString("C");
#endif
return 0;
}
else if (value > CENTER_ELEVATOR) // stick up
{
#if (DEBUG_LCD)
LCD_Position(1,5);
LCD_PrCString("U");
#endif
return ((float)value - CENTER_ELEVATOR) / (float)(MAX_ELEVATOR - CENTER_ELEVATOR);
}
else if (value < CENTER_ELEVATOR) // stick down
{
#if (DEBUG_LCD)
LCD_Position(1,5);
LCD_PrCString("D");
#endif
return -(CENTER_ELEVATOR - (float)value) / (float)(CENTER_ELEVATOR - MIN_ELEVATOR);
}
// action finished, clear flag to avoid doing it again
FlagsElevator &= ~DATA_AVAILABLE_ELEVATOR;
}
return 0;
}
// normalization function for Aileron (Timer1)
float EvaluateAileron(DWORD value)
{
// Check if pulsewidth data is available
if(FlagsAileron & DATA_AVAILABLE_AILERON)
{
#if (DEBUG_LCD)
LCD_Position(0,0);
LCD_PrHexInt(value);
#endif
// stick in center
if (Within(value, CENTER_AILERON, MARGIN_AILERON))
{
#if (DEBUG_LCD)
LCD_Position(0,5);
LCD_PrCString("C");
#endif
return 0;
}
else if (value < CENTER_AILERON) // stick left
{
#if (DEBUG_LCD)
LCD_Position(0,5);
LCD_PrCString("L");
#endif
return ((float)value - CENTER_AILERON) / (float)(MAX_AILERON - CENTER_AILERON);
}
else if (value > CENTER_AILERON) // stick right
{
#if (DEBUG_LCD)
LCD_Position(0,5);
LCD_PrCString("R");
#endif
return -(CENTER_AILERON - (float)value) / (float)(CENTER_AILERON - MIN_AILERON);
}
// action finished, clear flag to avoid doing it again
FlagsAileron &= ~DATA_AVAILABLE_AILERON;
}
return 0;
}
void switchDown()
{
//Gather current count
currentCount = Timer_wReadTimerSaveCV();
//3 seconds clocked at .0001 sec per cycles is 30000 timer ticks
if(currentCount >= 30000)
{
//Reset timer
Timer_WriteCompareValue(0);
}
else
{
//Check for previous values
if(dataAvailable != 0)
{
//Data has been collected - collect second value
time2 = currentCount;
//Data is no longer available since the second value has been collected
dataAvailable = 0;
//Let user know
LCD_Position(1,0);
LCD_PrCString("2");
//Reset the timer for next presses
Timer_WriteCompareValue(0);
}
else
{
//Data has not been collected - collect first value
time1 = currentCount;
//Let user know
LCD_Position(1,0);
LCD_PrCString("1");
}
}
//Write the difference in times to the LCD
LCD_Position(0,0);
LCD_PrHexInt(time1 - time2); //Down count so 1-2
}
void PWM10ms(void)
{
//Clear the pending interrupt
INT_CLR1 &= ~0b00000001;
//Increment count between 0 and 4 (5 resets to 0)
ten_ms_counter = ten_ms_counter + 1;
if(ten_ms_counter>=5)
{
ten_ms_counter = 0;
}
//Check state of count
if(ten_ms_counter <= 0)
{
//Low output (0% duty cycle)
bPulseWidth = (char)0;
}
else if(ten_ms_counter == 1)
{
//High output (100% duty cycle)
bPulseWidth = (char)255;
}
else if(ten_ms_counter == 2)
{
//Low
bPulseWidth = (char)0;
}
else if(ten_ms_counter >= 3)
{
//High
bPulseWidth = (char)255;
}
else
{
//Error
LCD_Position(0,0);
LCD_PrCString("Error!");
}
PWM_WritePulseWidth(bPulseWidth);
}
void main(void)
{
//Note: Button is on P1[0] and sequence in is on P0[0]
//Enable global interrupts
M8C_EnableGInt;
//CPU_F |= 0b00000001;
//Enable interrupts for GPIO (switch)
INT_MSK0 |= 0b00100000;
//Start LCD for debug
LCD_Start();
LCD_Init();
LCD_Position(0,0);
LCD_PrCString("Start...");
while(1)
{
}
}
void RegTimeView_Update(void)
{
BYTE startMin10, startMin, endMin10, endMin, startHour10, startHour, endHour10, endHour;
Config conf;
Persist_LoadConfig(&conf);
if(Keypad_IsKeyStored())
{
switch(Keypad_GetKey())
{
case 0x88:
// "1"
// start h up
if(conf.registerStart.hour < 23)
{
conf.registerStart.hour++;
}
Persist_SaveConfig(&conf);
break;
case 0x84:
// "2"
// start min up
if(conf.registerStart.min < 59)
{
conf.registerStart.min++;
}
Persist_SaveConfig(&conf);
case 0x48:
// "4"
// start h down
if(conf.registerStart.hour > 0)
{
conf.registerStart.hour--;
}
Persist_SaveConfig(&conf);
break;
case 0x44:
// "5"
// start min down
if(conf.registerStart.min > 0)
{
conf.registerStart.min--;
}
Persist_SaveConfig(&conf);
break;
case 0x28:
// "7"
// end h up
if(conf.registerEnd.hour < 23)
{
conf.registerEnd.hour++;
}
Persist_SaveConfig(&conf);
break;
case 0x24:
// "8"
// end min up
if(conf.registerEnd.min < 59)
{
conf.registerEnd.min++;
}
Persist_SaveConfig(&conf);
break;
case 0x18:
// "*"
// end h down
if(conf.registerEnd.hour > 0)
{
conf.registerEnd.hour--;
}
Persist_SaveConfig(&conf);
break;
case 0x14:
// "0"
// end min down
if(conf.registerEnd.min > 0)
{
conf.registerEnd.min--;
}
Persist_SaveConfig(&conf);
break;
case 0x11:
// "D"
MainView_Callback();
return;
break;
}
}
decToBcd(&startHour10, &startHour, conf.registerStart.hour);
startHour = (startHour10 << 4) | startHour;
decToBcd(&startMin10, &startMin, conf.registerStart.min);
startMin = (startMin10 << 4) | startMin;
decToBcd(&endHour10, &endHour, conf.registerEnd.hour);
endHour = (endHour10 << 4) | endHour;
decToBcd(&endMin10, &endMin, conf.registerEnd.min);
endMin = (endMin10 << 4) | endMin;
LCD_Position(0, 0);
LCD_PrCString("Inicio: ");
LCD_PrHexByte(startHour);
LCD_PrCString(":");
LCD_PrHexByte(startMin);
LCD_Position(1,0);
LCD_PrCString("Fin: ");
LCD_PrHexByte(endHour);
LCD_PrCString(":");
LCD_PrHexByte(endMin);
}
void main(void)
{
M8C_EnableGInt ; // Uncomment this line to enable Global Interrupts
M8C_EnableIntMask(INT_MSK1, INT_MSK1_DBB01); // Enable DBB01 Interrupt for TempCounter
M8C_EnableIntMask(INT_MSK1, INT_MSK1_DBB11); // Enable DBB01 Interrupt for MotorDriver
M8C_EnableIntMask(INT_MSK0, INT_MSK0_GPIO); // Enable GPIO interrupt for Tout
// Start the UART(with no parity), LCD, TempCounter and MotorDriver
UART_Start(UART_PARITY_NONE);
LCD_Start();
TempCounter_EnableInt(); // Enable interrupts for counter
TempCounter_Start();
MotorDriver_EnableInt(); // Enable interrupts for counter
// Start I2CHW
I2CHW_Start();
I2CHW_EnableMstr();
I2CHW_EnableInt();
WriteI2C(slaveAddress, 0xAC, 1, 0x02); // Write to access config, sets mode to cooling(POL = 1), also turns 1-SHOT off, continuous conversions
WriteI2C(slaveAddress, 0xA1, 2, (setTemp + tolerance), 0x00); // Sets initial high temp to be setTemp + tolerance
WriteI2C(slaveAddress, 0xA2, 2, (setTemp - tolerance), 0x00); // Sets initial low temp to be setTemp - tolerance
WriteI2C(slaveAddress, 0xEE, 0); // This tells the temperature IC to start converting the temperatures
// Writes initial string to LCD. When LCD is updated, only the numbers will be changed
LCD_Position(0,0); LCD_PrCString("CUR: 00 OFF ");
LCD_Position(1,0); LCD_PrCString("SET: 00 FAN OFF ");
// This is the command usage string
UART_CPutString("#################### Heating/Cooling Stepper Motors ##################\r\n\
# S ##\r\n\
# S - Set the desired Temperature\r\n\
# ## - Desired temperature in celsius\r\n\
#\r\n\
# T ##\r\n\
# T - Set the desired tolerance\r\n\
# ## - Desired tolerance in celsius\r\n\
#\r\n\
# M X\r\n\
# M - Change the mode of the thermostat\r\n\
# X - C is for cool, H is for heat, F is for off\r\n\
#\r\n\
# F X S\r\n\
# F - Change the mode of the fan\r\n\
# X - A is for automatic fan control, M is for always on\r\n\
# S - Speed of the fan, H = high, M = medium, L = low\r\n\
#####################################################################\r\n");
while (1)
{
char *cmd;
char *params;
if (GetLine(buf, &strPos, 79)) // Only process the data if GetLine returns true
{
cmd = Lowercase(cstrtok(buf, " ")); // Lowercase the first word from the inputted string
if (strlen(cmd) == 1 && cmd[0] == 's') // If the person entered s
{
int temp;
params = cstrtok(0x00, " "); // Read next word
// If next word isnt number or isnt 1 or 2 characters long, then return error
if (!IsNumber(params) || strlen(params) < 1 || strlen(params) > 2 || csscanf(params, "%d", &temp) != 1) goto error;
// If there is additional data at end of string or if number is not within 0-99, return error
if (cstrtok(0x00, " ") != 0x00) goto error;
if ( temp > 99 || temp < 0) goto error;
setTemp = temp;
WriteI2C(slaveAddress, 0xA1, 2, (setTemp + tolerance), 0x00); // Sets high temp to be setTemp + tolerance
WriteI2C(slaveAddress, 0xA2, 2, (setTemp - tolerance), 0x00); // Sets low temp to be setTemp - tolerance
updateLCD = TRUE; // Update the LCD
}
else if (strlen(cmd) == 1 && cmd[0] == 't') // If the person entered t
{
int tol;
params = cstrtok(0x00, " "); // Read next word
// If next word isnt number or isnt 1 or 2 characters long, then return error
if (!IsNumber(params) || strlen(params) < 1 || strlen(params) > 2 || csscanf(params, "%d", &tol) != 1) goto error;
// If there is additional data at end of string or if number is not within 0-10, return error
if (cstrtok(0x00, " ") != 0x00) goto error;
if (tol < 0 || tol > 10) goto error;
tolerance = tol;
WriteI2C(slaveAddress, 0xA1, 2, (setTemp + tolerance), 0x00); // Sets high temp to be setTemp + tolerance
WriteI2C(slaveAddress, 0xA2, 2, (setTemp - tolerance), 0x00); // Sets low temp to be setTemp - tolerance
updateLCD = TRUE; // Update the LCD
}
else if (strlen(cmd) == 1 && cmd[0] == 'm') // If the person entered m
{
char mode;
params = cstrtok(0x00, " "); // Read next word
// If next word isnt 1 character long, return error
if (strlen(params) != 1 || csscanf(params, "%c", &mode) != 1) goto error;
// If there is additional data at end of string, return error
if (cstrtok(0x00, " ") != 0x00) goto error;
mode = tolower(mode); // Lowercase the character
switch (mode)
{
case 'h':
thermostatMode = 1; // Set mode to heating
WriteI2C(slaveAddress,0xAC, 1, 0x00); // Change access config on DS1621 to heating(POL = 0)
break;
case 'c':
thermostatMode = 2; // Set mode to cooling
WriteI2C(slaveAddress, 0xAC, 1, 0x02); // Change access config on DS1621 to cooling(POL = 1)
break;
case 'f':
thermostatMode = 0; // Set mode to off
break;
default:
goto error; // Invalid character entered, goto error
}
CheckFan(); // Check the fan to see if it should be on
}
else if (strlen(cmd) == 1 && cmd[0] == 'f') // If the person entered f
{
char mode;
char speed;
params = cstrtok(0x00, " "); // Read next word
// If next word isnt 1 character long, then return error
if (strlen(params) != 1 || csscanf(params, "%c", &mode) != 1) goto error;
params = cstrtok(0x00, " "); // Read next word
// If next word isnt 1 character long, then return error
if (strlen(params) != 1 || csscanf(params, "%c", &speed) != 1) goto error;
// If there is additional data at end of string, return error
if (cstrtok(0x00, " ") != 0x00) goto error;
speed = tolower(speed); // Lowercase the speed and mode characters entered
mode = tolower(mode);
switch (mode)
{
case 'm':
fanMode = 0; // Set fan mode to manual
break;
case 'a':
fanMode = 1; // Set fan mode to automatic
break;
default: // Otherwise go to error
goto error;
}
MotorDriver_Stop(); // Stop the motor to change the period values
switch (speed)
{
case 'l':
fanSpeed = 0; // Set fan speed to low
MotorDriver_WritePeriod(49999); // See report for where these numbers came from
MotorDriver_WriteCompareValue(25000);
break;
case 'm':
fanSpeed = 1; // Set fan speed to medium
MotorDriver_WritePeriod(9999); // See report for where these numbers came from
MotorDriver_WriteCompareValue(5000);
break;
case 'h':
fanSpeed = 2; // Set fan speed to high
MotorDriver_WritePeriod(1999); // See report for where these numbers came from
MotorDriver_WriteCompareValue(1000);
break;
default: // Otherwise go to error if invalid input entered
goto error;
}
CheckFan(); // Check the fan to see if it should be on
}
else
goto error;
}
if (checkTemp) // Check the temperature
{
char buf[2];
ReadI2C(slaveAddress, 0xAA, 2, buf); // Read the temperature from IC, returns 2 bytes
curTemp = buf[0]; // We just care about the first byte
checkTemp = FALSE; // Turn flag off so it doesnt keep doing this
}
if (updateLCD) // Update the LCD
{
char buf[3];
NumToStr(buf, curTemp, 2); // Convert current temp to str
LCD_Position(0, 5); LCD_PrString(buf); // Print it
LCD_Position(0, 8);
switch(thermostatMode) // Print thermostat mode
{
case 0: LCD_PrCString("OFF "); break;
case 1: LCD_PrCString("HEAT"); break;
case 2: LCD_PrCString("COOL"); break;
}
NumToStr(buf, setTemp, 2); // Convert set temp to str
LCD_Position(1, 5); LCD_PrString(buf); // Print it
LCD_Position(1, 12);
if (fanMode == 1 && thermostatMode == 0) LCD_PrCString("OFF"); // Print current fan state
else if (fanSpeed == 0) LCD_PrCString("LOW");
else if (fanSpeed == 1) LCD_PrCString("MED");
else if (fanSpeed == 2) LCD_PrCString("HI ");
updateLCD = FALSE;
}
continue;
error:
UART_CPutString("# Invalid format entered. Valid formats are:\r\n\
# S ##\r\n\
# S - Set the desired Temperature\r\n\
# ## - Desired temperature in celsius\r\n\
#\r\n\
# T ##\r\n\
# T - Set the desired tolerance\r\n\
# ## - Desired tolerance in celsius\r\n\
#\r\n\
# M X\r\n\
# M - Change the mode of the thermostat\r\n\
# X - C is for cool, H is for heat, F is for off\r\n\
#\r\n\
# F X S\r\n\
# F - Change the mode of the fan\r\n\
# X - A is for automatic fan control, M is for always on\r\n\
# S - Speed of the fan, H = high, M = medium, L = low\r\n\
#####################################################################\r\n");
}
}
void main()
{
//Enable global interrupts
M8C_EnableGInt;
//Start LCD
LCD_Start();
//Init LCD
LCD_Init();
//Iniitalize pulsewidth
FanPWM_WritePulseWidth(255);
//Write label to LCD
LCD_Position(0,0);
LCD_PrCString("Pulse Width: ");//Begin writing at 0,13
LCD_Position(0,13);
LCD_PrHexByte(FanPWM_bReadPulseWidth());
//Start Fan PWM
FanPWM_Start();
//Enable FanPWM interrupt
FanPWM_EnableInt();
while(1)//control loop
{
//Wait for sleep timer
while(INT_CLR0 & 0b01000000 == 0){}//Wait for posted sleep timer interrupt
//Clear sleep timer interrupt
INT_CLR0 &= ~0b01000000;
//Read switch input 3 times (debounce)
if(PRT1DR & 0b00000001 == 0b00000001) //First read
{
if(PRT1DR & 0b00000001 == 0b00000001) //Second read
{
if(PRT1DR & 0b00000001 == 0b00000001) //Third read - Button has been pressed
{
LCD_Position(1,0);
LCD_PrCString("Waitiing...");
while(PRT1DR & 0b00000001 == 0b00000001){}//Wait for button to be released
LCD_Position(1,0);
LCD_PrCString(" ");
//Decrement pulse width
FanPWM_WritePulseWidth(FanPWM_bReadPulseWidth() - 1);
//Update display
LCD_Position(0,13);
LCD_PrHexByte(FanPWM_bReadPulseWidth());
}
}
}
}//end control loop
}
void main(void)
{
// Enable Global Interrupt
M8C_EnableGInt;
// Clear the flags
FlagsElevator = 0;
FlagsAileron = 0;// new for motorcontroll2
FlagUltrasoon = 0;
// Start timers and enable interrupt
Timer1_Start();
Timer1_EnableInt();
Timer2_Start();// new for motorcontroll2
Timer2_EnableInt();// new for motorcontroll2
Timer3_Start();
Timer3_EnableInt();
TriggerUltrasoon();
// Init motors
PWM1_Start();
PWM2_Start();
#if (DEBUG_LCD)
LCD_Start();
#endif
while (TRUE)
{
float aileronNormalized,
elevatorNormalized;
float distance;
float speed, direction;
float motorLeft, motorRight;
BOOL forward;
aileronNormalized = EvaluateAileron(PulseWidthAileron);
direction = fabs(aileronNormalized);
elevatorNormalized = EvaluateElevator(PulseWidthElevator);
speed = fabs(elevatorNormalized);
forward = (elevatorNormalized >= 0);
distance = EvaluateUltrasoonSensor();
if (distance < MIN_SAFE_DISTANCE)
{
if (forward)
speed = 0;
}
motorLeft = speed; // default is straight forward
motorRight = speed;
if (aileronNormalized < 0) // turning left
{
motorRight = speed;
motorLeft = speed * (1 - direction);
}
else if (aileronNormalized > 0) // turning right
{
motorLeft = speed;
motorRight = speed * (1 - direction);
}
if (forward)
{
// ccw
PRT1DR |= 0x08; // AIN1
PRT1DR &= ~0x02; // AIN2
PRT1DR |= 0x20; // BIN1
PRT1DR &= ~0x80; // BIN2
}
else
{
//cw
PRT1DR &= ~0x08; // AIN1
PRT1DR |= 0x02; // AIN2
PRT1DR &= ~0x20; // BIN1
PRT1DR |= 0x80; // BIN2
}
// Denormalize to Engine
motorLeft *= (MAX_POWER - MIN_POWER);
motorLeft += MIN_POWER;
motorRight *= (MAX_POWER - MIN_POWER);
motorRight += MIN_POWER;
PWM1_WritePulseWidth(motorLeft);
PWM2_WritePulseWidth(motorRight);
#if (DEBUG_LCD)
LCD_Position(0,7);
LCD_PrHexInt(motorLeft);
LCD_Position(1,7);
LCD_PrHexInt(motorRight);
LCD_Position(0,12);
LCD_PrCString(forward ? "F" : "B");
#endif
}
}
