Programmazione ROBIPIC in linguaggio C
Area di Progetto 2008/2009 Classe 4AT
Allievi: Mazzucco Luca, Zambon Mattia, Martini Matteo, Della Libera Riccardo
Video del robot presente anche su youtube:
http://www.youtube.com/watch?v=dpHIbVlTh2s
ROBOPIC
Pagina 1 di 6
Scopo dell’esperienza :
Controllo della corsa del robopic seguendo un percorso nero su
fondo bianco con inversione di marcia al riconoscimento di una
traccia a righe b/n.
L’hardware utilizzato prevede un kit di montaggio Robopic basato su microcontrollore PIC
16F887 che può essere programmato via USB con una scheda PICKIT2 di Microchip.
Gli accessori montati sulla scheda ed utilizzati per il progetto sono:
- display LCD 2x16
- autoparlante piezo
- leds segnalatori
- 2 driver per i motori DC
- 2 sensori IR per rilevare il colore del percorso
Il software di programmazione utilizza il linguaggio C tramite MickroC.
/*
PROGRAMMA PER FAR SEGUIRE AL ROBOT LA PISTA NERA CON FUNZIONE DI INVERSIONE DI MARCIA QUANDO TROVA 3
RIGHE NERE TRASVERSALI
ATTENZIONE! si ricorda che...
->Le velocità possono cambiare e le funzioni non eseguite correttamente se le batterie non sono
cariche.
->Bisogna usare un nero intenso perché i sensori lo vedano come nero (es. nastro isolante)
-> Soglia del bianco per entrambi i sensori >400, soglia del nero per entrambi i sensori <270
-> Soglia nel caso delle curve con controllo di un solo sensore sul nero <200
*/
#include <motor.h>
//funzioni azionamento motore
//dichiarazione variabili
int righe, cont, suono, j, Sensor0, Sensor1, y, curve=1, nero=0, dim, nofor, riparti, i;
//Funzione lettura sensori
void Read_Adc()
{
ADCON0=0b11000001;
ADCON0.GO=1;
while(ADCON0.GO);
Sensor0=(ADRESH*4)+(ADRESL/64);
ADCON0=0b11000101;
ADCON0.GO=1;
while(ADCON0.GO);
Sensor1=(ADRESH*4)+(ADRESL/64);
}
//
//
//
//
//
//
//
//
Seleziona quale uscita
Coverte il segnale
Aspetta che il segnale
Porta il segnale letto
Seleziona quale uscita
Converte il segnale
Aspetta che il segnale
Porta il segnale letto
leggere del porto
//
//
//
//
Aspetta 1 secondo per partire
Dichiaro che il PORT A è tutto analogico
Dichiaro che il PORT A è tutto un ingresso
Inizializzo il cicalino
venga convertito
in numero intero
leggere del porto
venga convertito
in numero intero
//programma principale
void main()
{
Delay_ms(1000);
ANSEL = 0xFF;
TRISA = 0xFF;
Sound_Init(&PORTC,0);
//inizializzo le variabili
y=0;
//non faccio inversione
righe=0;
//azzero conteggio righe
curve=1;
//attivo le curve
nero=0;
//non sono su linee nere
nofor=0;
riparti=0;
//non devo accelerare piano
//loop infinito
while(1)
{
Read_Adc(); //leggo sensori
ROBOPIC
Pagina 2 di 6
if ((Sensor0>400)&(Sensor1>400)) //se vedo tutto bianco
{
//se devo accelerare gradualmente
if(riparti==1){
//se verso destra
for(dim=0;dim<231;dim++){
gira_m(dim,230);
gira_m(dim,230);
gira_m(dim,230);
}
}
if(riparti==2){
//se verso sinistra
for(dim=0;dim<231;dim++){
gira_m(dim,230);
gira_m(dim,230);
gira_m(dim,230);
}
}
riparti=0; //al prossimo giro continuo con la velocità attuale
//se è disattivo il controllo sulle curve avanza
if (curve==1){
Forward(230);
//avanti veloce
}else{
Forward(210);
//avanti non troppo veloce
}
cont=0;
nofor=0;
più lentamente per contare le
righe
}
if ((Sensor0<270)&(Sensor1<270)) // se vedo tutto nero
{
nofor=0;
if (nero==0){
//ricontrollo se è tutto nero, per sicurezza
Read_Adc();
if((Sensor0<220)&(Sensor1<220)){ //guardo se è tutto nero
righe++; //conto le righe
nero=1; //disattivo il controllo se è una riga nera al prossimo giro
y=0;
curve=0; //disattivo le curve, altrimenti sul conteggio può sterzare
}else{
//se non è tutto nero, allora è una curva
nero=0;
curve=1;
riparti=0;
goto nonero;
//va a guardare se è una curva destra o sinistra
}
}else{
y=0;
curve=0;
//disattivo le curve
if(cont==0){
cont=1;
righe++;
}
Forward(200);
//se è sopra le righe nere, va più piano
}
}
nonero:
if (Sensor0<200 & curve==1 & nero==0) //guardo se è curva destra o sinistra
{
righe=0;
//se è una curva, non ci sono righe nere
if(nofor==0){
for(dim=230;dim>120;dim--){ //faccio un rallentantamento graduato
gira_m(dim,230);
gira_m(dim,230);
gira_m(dim,230);
gira_m(dim,230);
}
riparti=2;
}
nofor=1;
gira_m(230,0); //dopo aver rallentato, proseguo con la curva
righe=0;
//se è una curva, non ci sono righe nere
y=0;
//non devo fare inversione di marcia perchè è una curva
}
Read_Adc();
//rileggo sensori
if (Sensor1<200 & curve==1 & nero==0)//guardo se è curva destra o sinistra
{
//stesse funzioni della precedente ma per curva inversa
righe=0;
if(nofor==0){
for(dim=230;dim>120;dim--){
gira_m(230,dim);
gira_m(230,dim);
gira_m(230,dim);
ROBOPIC
Pagina 3 di 6
gira_m(230,dim);
}
riparti=1;
}
nofor=1;
gira_m(0,230);
righe=0;
y=0;
}
if (righe==4){
Sound_Play(160,1000);
Forward(250);
delay_ms(800);
//quando ho contato tutte le righe nere
//suono
//proseguo velocemente...
//...per 800ms
tsu:
Forward(250);
Sound_Play(120,500);
//proseguo e suono
if(Sensor0<300 & Sensor1<300){ //appena leggo nero, fermo il motore
motor_stop();
goto tgiu;
// faccio inversione
}
goto tsu;
//continuo ad andare avanti finchè non vedo nero
tgiu:
y=1;
}
if (y==1){
//faccio inversione
delay_ms(100);
//attendo 100ms
S_Right(235);
//faccio ruotare il robot...
delay_ms(500);
//... per 500ms senza leggere sensori
Read_Adc();
//rileggo
while(Sensor0>300){
//finchè rimango sul bianco, continuo a girare
S_Right(235);
Read_Adc();
}
Read_Adc();
//rileggo
while(Sensor0<300 | Sensor1<300){
S_Right(230);
//quando vedo nero, inizio a rallentare
Read_Adc();
}
motor_stop();
//appena vedo ancora bianco, fermo il motore
delay_ms(100);
//aspetto 100ms
Forward(250);
//vado avanti per 1 sec
delay_ms(1000);
y=0;
//disattivo la funzione dell'inversione
righe=0;
//azzero il conteggio delle righe
curve=1;
//riattivo le curve
nero=0;
//non siamo su linea nera
//proseguo normalmente con il programma
}
}
}
Con il seguente file header per il motore motor.h
char motor_duty_= 127;
char motor_init_=0;
//
//
//
//
*** Motor A
PD0 ====>
PD1 ====>
PC2 ====>
*****
1A
1B
1E (PWM1)
//
//
//
//
*** Motor B
PB1 ====>
PB2 ====>
PC1 ====>
*****
2A
2B
2E (PWM2)
// Defalt PWM 50%
// Status initial
//****************************************************
//********** Initial Motor Function ******************
//****************************************************
void Motor_Init()
{
if (motor_init_==0)
// First time ?
{
motor_init_=1;
// Status
ANSELH.F0=0;
// RB1 ==> Digital IO
ANSELH.F2=0;
// RB2 ==> Digital IO
TRISB.F1=0;
// Motor B 2A
TRISB.F2=0;
// Motor B 2B
ROBOPIC
Pagina 4 di 6
TRISD.F0=0;
TRISD.F1=0;
Pwm1_Init(5000);
Pwm2_Init(5000);
//
//
//
//
Motor A
MOtor A
Initail
Initail
1A
1B
PWM 1E
PWM 2E
}
}
//****************************************************
//****************************************************
//********** Control Duty Cycle *********************
//****************************************************
void Change_Duty(char speed)
{
if (speed != motor_duty_)
// Check Same old speed
{
motor_duty_=speed;
// Save for old speed
Pwm1_Change_Duty(speed);
// Motor A
Pwm2_Change_Duty(speed);
// Motor B
}
}
void gira_piano(char speed1, char speed2)
{
if (speed1 != motor_duty_)
// Check Same old speed
{
motor_duty_=speed1;
// Save for old speed
Pwm1_Change_Duty(speed1);
// Motor A
Pwm2_Change_Duty(speed2);
// Motor B
}
}
//****************************************************
/********** Motor A Forward ********/
void Motor_A_FWD()
{
Pwm1_Start();
PORTD.F0 =0;
PORTD.F1 =1;
}
/************************************/
/********** Motor B Forward ********/
void Motor_B_FWD()
{
Pwm2_Start();
PORTB.F1 =0;
PORTB.F2 =1;
}
/************************************/
/********** Motor A Backward *******/
void Motor_A_BWD()
{
Pwm1_Start();
PORTD.F0 =1;
PORTD.F1 =0;
}
/************************************/
/********** Motor B Backward *******/
void Motor_B_BWD()
{
Pwm2_Start();
PORTB.F1 =1;
PORTB.F2 =0;
}
/************************************/
/********** Motor A Off ************/
void Motor_A_Off()
{
Pwm1_Stop();
PORTD.F0 =0;
PORTD.F1 =0;
}
/************************************/
/********** Motor B Off
void Motor_B_Off()
{
Pwm2_Stop();
PORTB.F1 =0;
PORTB.F2 =0;
}
ROBOPIC
************/
Pagina 5 di 6
/************************************/
/********** Go Forward
************/
void Forward(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_FWD();
Motor_B_FWD();
}
/************************************/
/********** Go Backward ************/
void Backward(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_BWD();
Motor_B_BWD();
}
/************************************/
/********** Spin Left
*************/
void S_Right(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_FWD();
Motor_B_BWD();
}
/************************************/
/********** Spin Right
************/
void S_Left(char speed)
{
Motor_Init();
Change_Duty(speed);
Motor_A_BWD();
Motor_B_FWD();
}
/************************************/
/********** Spin Right
************/
void S_Sin(char speed1,char speed2)
{
Motor_Init();
gira_piano(speed1,speed2);
Motor_A_FWD();
Motor_B_FWD();
}
/************************************/
/********** Stop Motor
************/
void Motor_Stop()
{
Motor_Init();
Change_Duty(0);
Motor_A_Off();
Motor_B_Off();
}
/************************************/
ROBOPIC
Pagina 6 di 6