void main(void)
{
Sys_Init();
putchar(' ');
XBR0_Init();
SMB_Init();
PCA_Init();
Drive_Init();
Port_Init();
ADC_Init();
ranger_pd_init();
//fan angle initialization code goes here
while(1)
{
voltage_update();
if(SS)
{
Range_Update(); //update the range
Drive_Motor(ranger_pd());
}
else
Drive_Motor(PW_NEUT); //if ss is not flipped, put it in neutral
}
}
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 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 voltage_update()
{
voltage = findvoltage();
if (voltage < MIN_VOLT)
{
Drive_Motor(PW_MIN);
while(1){}
}
}
void Ranger(void) {
if (new_range){ //80ms passed
printf("\r\n new range");
new_range = 0;
range=ReadRanger();
printf("\r\nrange2: %d", range);
comp_and_range_Data[0] = 0x51 ; // write 0x51 to reg 0 of the ranger:
i2c_write_data(0xE0, 0, comp_and_range_Data, 1) ; // write one byte of data to reg 0 at addr_r
}//end if new range
Drive_Motor(); //even if not new range info, run the drive motor
}//end ranger
//------------------------------------------------------------------------------------
//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 init_sensors()
{
int a;
OCR1B=160;
OCR1A=160;
Drive_Motor(1,2);
for(int i=0;i<90;i++)
{
_delay_ms(10);
a=adc_start(1);
l1=(a<l1)?a:l1;
a=adc_start(2);
l2=(a<l2)?a:l2;
a=adc_start(3);
l3=(a<l3)?a:l3;
}
Drive_Motor(2,1);
_delay_ms(300);
for(int i=0;i<90;i++)
{
_delay_ms(10);
a=adc_start(1);
u1=(a>u1)?a:u1;
a=adc_start(2);
u2=(a>u2)?a:u2;
a=adc_start(3);
u3=(a>u3)?a:u3;
}
Drive_Motor(1,2);
while(adc_start(2)<(u2-20));
Drive_Motor(0,0);
u1-=l1;
u2-=l2;
u3-=l3;
}
int main()
{
init();
float kp=1.5;
float kd=2.5;
int error=0, prev_error=0, rate=0, pwm_duty, integ=0;
int v1=0, v2=0, v3=0, s1, s2,a,b;
while(bit_is_set(PIND,3));
init_sensors();
while(1)
{
a=1;
b=1;
v1=adc_start(1);
v2=adc_start(2);
v3=adc_start(3);
v1=(v1-l1);
v2=(v2-l2);
v3=(v3-l3);
error=-((u1-v1)-(u3-v3));
rate=error-prev_error;
prev_error=error;
integ+=0.001*error;
if (error>0)
PORTB = (1<<0) | (0<<1);
else if (error<0)
PORTB = (1<<1) | (0<<0);
pwm_duty=error/kp+rate/kd+integ;
s1=240+pwm_duty;
s2=240-pwm_duty;
if (s1>255)
{
s2=s2-(s1-255);
s1=255;
}
if (s2>255)
{
s1=s1-(s2-255);
s2=255;
}
if (s1<0)
{
s1=0;
}
if (s2<0)
{
s2=0;
}
if (v1<45 && v2<45 && v3<45)
{
s1=180;
s2=180;
a=2;
Drive_Motor(0,0);
break;
}
OCR1B=s1;
OCR1A=s2;
Drive_Motor(a,b);
}
return 0;
}
