void Ultrasonic_Ranger() {
if(new_range)
{
new_range = 0;
range = read_ranger();
start_ping();
printf("\rRange: %d\n", range);
//set motor
if(range < 40) //forward
{
if(range > 10)
PW_SC = PW_MAX_SC - (PW_MAX_SC - PW_NEUTRAL_SC)*(range-10)/30.0;
else
PW_SC = PW_MAX_SC;
}
else if(range > 50) //reverse
{
if(range < 90)
PW_SC = PW_MIN_SC + (PW_NEUTRAL_SC - PW_MIN_SC)*(1 - (range-50)/40.0);
else
PW_SC = PW_MIN_SC;
}
else //neutral
PW_SC = PW_NEUTRAL_SC;
printf("\rPW: %u\n", PW_SC);
PCA0CPL2 = 0xFFFF - PW_SC;
PCA0CPH2 = (0xFFFF - PW_SC) >> 8;
}
}
//------------------------------------------------------------------------------------
//ranger function
//------------------------------------------------------------------------------------
void Ranger(void) {
if (new_range) //80ms passed
{
//printf("/r/n new range");
new_range = 0;
range=read_ranger();
//printf("range2: %d", range);
Data1[0] = 0x51 ; // write 0x51 to reg 0 of the ranger:
i2c_write_data(0xE0, 0, Data1, 1) ; // write one byte of data to reg 0 at addr_r
}
Drive_Motor(); //even if not new range info, run the drive motor
}
void main(void) {
// initialize board
Sys_Init();
putchar(' '); //the quotes in this line may not format correctly
Port_Init();
PCA_Init();
SMB_Init();
Interrupt_Init();
printf("Starting\n\r");
//print beginning message
printf("Embedded Control Drive Motor Control\r\n");
// Initialize motor in neutral and wait for 1 second
MOTOR_PW = PW_NEUT;
motorPW = 0xFFFF-MOTOR_PW;
PCA0CPL2 = motorPW;
PCA0CPH2 = motorPW>>8;
printf("Pulse Width = %d\r\n",MOTOR_PW);
c = 0;
while (c < 50); //wait 1 second in neutral
printf("end wait \r\n");
//Main Functionality
while (1) {
if (!SS1) { // If the slide switch is active, set PW to center
PW = PWCENTER;
PCA0CP0 = 0xFFFF - PW; // Update comparator with new PW value
while (!SS1); // Wait...
} else if (take_heading) { // Otherwise take a new heading
reading = Read_Compass(); // Get current heading
printf("%d\n\r", reading);
Steering_Servo(reading); // Change PW based on current heading
PCA0CP0 = 0xFFFF - PW; // Update comparator with new PW value
}
if (getRange) {
getRange = 0; // Reset 80 ms flag
range_val = read_ranger(); // Read the distance from the ranger
printf("Range: %d cm \r\n", range_val);
printf("Pulse Width: %d \r\n", MOTOR_PW);
// Start a new ping
Data[0] = 0x51; // write 0x51 to reg 0 of the ranger:
i2c_write_data(addr, 0, Data, 1); // write one byte of data to reg 0 at addr
}
if (SS0) Drive_Motor(range_val);
else Drive_Motor(45); // Hold the motor in neutral if the slide switch is off
}
}
