//-----------------------------------------------------------------------------------
//heading and ranger functions
//-----------------------------------------------------------------------------------
void Heading(void) {
if (new_heading){ //20 ms passed
heading = ReadCompass();
new_heading = 0;
}//end if new heding
Steering_Servo(); //even if not new heading, run the steering servo
}//end heading
//********************************************************************
// Main Functions
//********************************************************************
void main(void) { //Main function
Sys_Init(); // initialize board
putchar(' '); //the quotes in this line may not format correctly
Port_Init();//Init ports
XBR0_Init();//init xbro
PCA_Init();//init pca
SMB_Init();//init smb
printf("\r\n\n\n\nEmbedded Control Electric Compass and Ranger\n"); //print beginning message
Calibration();//Run calibration
comp_cal(); //Compass calibration
Choose_heading(); //Heading choice
printf("\r\nheading error");
while(1) { //inf loop, 40 ms it returns the heading
if (new_heading){ //enough overflows for a new heading COMPASS STUFF
new_heading = 0;//Clear new_heading
heading = ReadCompass(); //get compass heading
Steering_Servo(); //run steer func
}//end if new heading
if (new_range) { //if 80 ms has passed
new_range=0;//Clear new_range
range=ReadRanger();//read ranger
start_ping();//start ping
counts++;//set up text function
Drive_Motor(); //run drive func
}//end if new_range
if (counts == 3){ //prevoudly output prined every 200 ms, now every 180 ms
print_output();//Print data function. Delete this if faster output is desired
}//end if counts
}//end inf while
}//end main
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
}
}
//-----------------------------------------------------------------------------
// Main Function
//-----------------------------------------------------------------------------
void main(void)
{
// initialize board
Sys_Init();
putchar(' '); //the quotes in this line may not format correctly
Port_Init();
XBR0_Init();
PCA_Init();
ADC_Init();
SMB_Init();
//drive motor config
printf("\rBegin Speed Controller Initialization\n");
PCA0CPL2 = 0xFFFF - PW_SC;
PCA0CPH2 = (0xFFFF - PW_SC) >> 8;
counts = 0;
while(counts < 100); //wait 1 second
printf("\rSC init finished\n");
//start_ping();
// set the PCA output to a neutral setting
PW_SS = PW_CENTER_SS;
PCA0CPL0 = 0xFFFF - PW_SS;
PCA0CPH0 = (0xFFFF - PW_SS)>>8;
/*
//print beginning message
printf("Embedded Control Steering Calibration\n");
printf ("\n\rr or l for direction, space for center, then right, then left\n\r");
while (flag < 3)
{
Steering_Config();
}
*/
//select heading/gain
desired_heading = pick_heading();
gain_SS = pick_gain();
while(1)
{
//steering servo
//ultrasonic ranger
if(!SS1){ //switch is on
Steering_Servo();
Ultrasonic_Ranger();
run_stop = 1;
}
else if(run_stop) {
PW_SC = PW_NEUTRAL_SC; //drive motor stop
PCA0CPL2 = 0xFFFF - PW_SC;
PCA0CPH2 = (0xFFFF - PW_SC) >> 8;
desired_heading = pick_heading();
gain_SS = pick_gain();
run_stop = 0;
}
if(new_display)
{
new_display = 0;
Print_LCD();
}
}
}
//-----------------------------------------------------------------------------
// Main Function
//-----------------------------------------------------------------------------
void main(void)
{
desired_range = 150;
// initialize board
Sys_Init();
putchar(' '); //the quotes in this line may not format correctly
PCA_Init();
ADC_Init();
Interrupt_Init();
Port_Init();
XBR0_Init();
SMB_Init();
putchar('\r');
//print beginning message
printf("En taro Adun, Executor!\r\n");
// set the PCA output to a neutral setting
steering_neutral();
ranger_neutral();
wait(); /* Let stuff warm up */
// Get the PD constants and target heading from the keypad
compass_kp = read_gain()/10.;
desired_heading = read_heading();
compass_kd = read_gain_high();
ranger_kp = read_gain_thrust()/10.;
ranger_kd = read_gain_thrust_high();
//Write the settings to the record
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
PCA0CPL1 = 0xFFFF - 3200;
PCA0CPH1 = ( 0xFFFF - 3200 ) >> 8 ;
// Start at time 0.
time = 0;
while(1) /* Infinite loop is go */
{
if ( nCounts == 50 )
{
// Update the LCD loop
lcd_clear();
lcd_print( "Batt level: %d\n", get_battery_voltage() );
lcd_print("Heading :%d\n", heading/10);
lcd_print("Alt: %d", range );
if (SS)
lcd_print("PAUSED\n");
else
lcd_print("\n");
lcd_print("# forHead, * forGain");
}
if ( read_keypad() != -1 )
{
// Check the keypad
pause();
if ( getKey() == '#' )
{
// If # was pressed, get a new heading
steering_neutral();
ranger_neutral();
desired_heading = read_heading();
time = 0;
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
}
else if( getKey() == '*' )
{
// If * was pressed, get new gain contants
steering_neutral();
ranger_neutral();
compass_kp = read_gain()/10.;
compass_kd = read_gain_high();
ranger_kp = read_gain_thrust()/10.;
ranger_kd = read_gain_thrust_high();
time = 0;
printf ( "Compass KP = %d, Compass KD = %d, Desired Heading = %d, Ranger KP = %d, Ranger KD = %d, Desired Range = %d\r\n",
compass_kp,
compass_kd,
desired_heading,
ranger_kp,
ranger_kd,
desired_range );
printf ( "Time, heading, range\r\n ");
}
}
else
pause();
if(SS) //while the SS is off
{
//printf("The slide switch is OFF \r\n");
steering_neutral();
ranger_neutral();
}
if(!SS) //SS is On
{
//printf("The Slide switch is On \r\n");
if ( new_heading )
printf("%lu,%d,%d\r\n",
time,
compass_old_error,
ranger_old_error );
if (new_heading) //wait for 40ms
{
heading=read_compass();
//printf("The heading result is %d\r\n", heading);
new_heading=0;
Steering_Servo( );
}
if (new_range)
{
ReadRanger( );
Speed_Cont( ); //adjust motor speed
new_range=0; //reset ranger flag
}
}
}
}
