void main()
{
initRobot();
while(1)
{
//lettura dello stato dei sensori
line_R = Adc_Read(5);
line_C = Adc_Read(6);
line_L = Adc_Read(7);
if(line_L<512 & line_C>512 & line_R<512)
{
// 0-1-0 vado dritto alla veocità di crociera
PWM1_Change_Duty(190);
PWM2_Change_Duty(190);
Delay_ms(5);
}
if(line_L<512 & line_C>512 & line_R>512)
{
// 0-1-1 deve accelerare la ruota sinistra
PWM1_Change_Duty(145);
PWM2_Change_Duty(190);
Delay_ms(5);
}
if(line_L<512 & line_C<512 & line_R>512)
{
// 0-0-1 deve accelerare la ruota sinistra
PWM1_Change_Duty(135);
PWM2_Change_Duty(210);
Delay_ms(5);
}
if(line_L>512 & line_C>512 & line_R<512)
{
// 1-1-0 deve accelerare la ruota destra
PWM1_Change_Duty(190);
PWM2_Change_Duty(145);
Delay_ms(5);
}
if(line_L>512 & line_C<512 & line_R<512)
{
// 1-0-0 deve accelerare la ruota destra
PWM1_Change_Duty(210);
PWM2_Change_Duty(135);
Delay_ms(5);
}
if(line_L<512 & line_C<512 & line_R<512)
{
// 0-0-0 fuori dalla linea, indietro
PWM1_Change_Duty(90);
PWM2_Change_Duty(90);
Delay_ms(5);
}
} //end main loop
} //end main()
float Adc_IR(int ch)
{
int k=0;
unsigned int raw = 0;
float value = 0;
raw = Adc_Read(ch);
for(k=1; k < NUMADC; k++)
{
if(adc_table[k][0] < raw)
{
value = (adc_table[k-1][0] - raw);
value = value / (adc_table[k-1][0] - adc_table[k][0]);
value = value + adc_table[k-1][1];
// value = adc_table[k-1][1] + (float)((raw - adc_table[k-1][0]) *
// (adc_table[k][1] - adc_table[k-1][1]) / (adc_table[k][0] - adc_table[k-1][0]));
break;
}
}
if (k == NUMADC)
{
value = adc_table[k-1][1];
}
if (value < 4)
{
value = 3;
}
else if(value > 30)
{
value = 30;
}
return value;
}
void MainLoop_GetReading(void * param)
{
getReadingParameterStruct * parameterStruct;
int8_t adcReturnCode;
int16_t adcReading;
float reading;
RETURN_IF_NULL(param);
parameterStruct = (getReadingParameterStruct *)param;
RETURN_IF_NULL(parameterStruct->outputModel);
RETURN_IF_NULL(parameterStruct->ledDisplay);
RETURN_IF_NULL(parameterStruct->getReadingAlarm);
if (getConversionStatus() == ADC_CONVERSION_BUSY)
{
return;
}
adcReturnCode = Adc_Read(&adcReading);
if (adcReturnCode != ADC_READ_SUCCESS)
{
return;
}
reading = LineFit_GetOutput(parameterStruct->outputModel, adcReading);
LedNumber_SetNumber(parameterStruct->ledDisplay, round_int16(reading));
TimeService_DeactivatePeriodicAlarm(parameterStruct->getReadingAlarm);
}
long temp_sensor()
{
Vin = Adc_Read(2); // Read from channel 2 (AN2)
Vin = 4.88*Vin; // Scale up the result
Vin = Vin /10; // Convert to temperature in C
return Vin;
}
unsigned char Adc_Average (char ch) //returns the average of a few reads of an ADC channel
{
int average = 10;
int sum = 0;
int i;
for (i = 0; i < average; i++)
{
sum = sum + Adc_Read(ch);
}
return sum/average;
}
void main()
{
unsigned char value, lastValue, CANdata[8];
/* These variables are used to check that the rear node and the
temperature sensing node are active. */
signed char WaitingPeriods, state202, state50, stateLight;
unsigned char c; /* A counter for doing things several times */
unsigned long Brake_light; /* 1 if brake light should be on, off=0 */
unsigned char error_flag=0;
long id;
unsigned short send_flag, dt, len, read_flag;
unsigned int total, V0,V1,V2,V3,tempX10;
send_flag = _CAN_TX_PRIORITY_0 &
_CAN_TX_NO_RTR_FRAME;
/* The state of the CANbus Messages can be:
0 or less : Message not received in the last WaitingPeriods periods
x [x from 520 to 1] : Message received (WaitingPeriods-x) periods ago
The states begin at 20: */
WaitingPeriods = 5;
state202 = 0;
state50 = 0;
stateLight = 0;
/* Get the CANbus ready */
CANbus_setup();
/* Set up and start the timing interrupts */
setup_interrupts();
TRISA =0xFF; /* PORTA is input */
ADCON1=0x80; /* Use supply as Vref. */
TRISC = 0; /* PORT C is for output */
PORTC= 0x10; /* Turn on a LED to show life */
/* Program loop.
*/
Brake_light=0;
for(;;) /* Endless loop */
{
if (Tcount20Hz>50){ /* Do this at 20 Hz, roughly */
/* Read the ADC channels
Channels 0 and 1 first */
V0=Adc_Read(0);
V1=Adc_Read(1);
total=V0+V1;
/* Now Channel 2, brake pedal */
V2=Adc_Read(2);
/* Repeat for Channel 3, brake light level setting pot. */
V3=Adc_Read(3);
/* Now decide if Brake light should be on. */
if (V2>V3) Brake_light=1;
else Brake_light=0;
/* Switch on-board LED for diagnostics */
PORTC.F5=Brake_light;
/* Transmit the accelerator and brake pedal settings */
value=V0/5; /* Scale to 0 to 200 from 0 to 1023 */
if (value>200) value=200;
if ((value==lastValue)&&(value>60)) c++;
else c=0;
/* If the throttle pos is stuck, and over 30% for more than
5*20= 120 loops, i.e. 5 seconds, then assume stuck. */
if (c>120) error_flag.F1=1;
else error_flag.F1=0;
if (c>130) c=130; /* Stop c reseting to 0 by going over 255 */
lastValue=value;
/* Now check pedals are not stuck. The total reading from both
pots should be 1023, as one falls as the other rises. Initially
allow 300 either side of this. */
if ((total<723)||(total>1323)) error_flag.F0=1;
else error_flag.F0=0;
/* Now set up the data for message 0x80 */
CANdata[0]=value;
/* Scale the brake pedal setting to 0-200 */
value=V2/5;
CANdata[1]=value;
CANdata[2]=error_flag;
id=0x80;
CANWrite(id, CANdata, 3, send_flag);
/* Transmit brake light message, which has id 0x200 */
CANdata[0]=Brake_light;
id=0x200;
CANWrite(id, CANdata, 1, send_flag);
Tcount20Hz=0;
} /* End of things done at 20Hz */
if (Tcount1Hz>1000){ /* Do this at 1 Hz, roughly */
if (state202==0){
error_flag.F6=1;
}
else{
state202--;
error_flag.F6=0;
}
if (state50==0){
error_flag.F7=1;
}
else{
state50--;
error_flag.F7=0;
}
if (stateLight==0)
error_flag.F3=1;
else{
stateLight--;
error_flag.F3=0;
}
Tcount1Hz=0;
}
dt = CANRead(&id, CANdata, &len, &read_flag);
if (dt>0){ /*Message received */
if (id==0x202){
state202=WaitingPeriods;
error_flag.F4=CANdata[0].F1;
error_flag.F5=CANdata[0].F2;
if (Brake_light == CANdata[0].F0)
stateLight=WaitingPeriods;
}
if (id==0x50){
state50=WaitingPeriods;
tempX10= (CANdata[0]<<8) + CANdata[1];
if (tempX10>600) error_flag.F2=1;
else error_flag.F2=0;
}
}
} /* End of endless loop */
} /* End of void main() */
