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 } }