main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "TCS3200.h"
00004 #include "STSpin240_250.h"
00005 
00006 STSpin240_250_init_t init =
00007  {
00008   20000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz             */
00009   20000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz             */
00010   20000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz              */
00011   50,    /* Duty cycle of PWM used for Ref pin (from 0 to 100)                  */
00012   TRUE   /* Dual Bridge configuration  (FALSE for mono, TRUE for dual brush dc) */
00013  };
00014 
00015 STSpin240_250 *motor;
00016 
00017 /*FUNZIONI:.*/
00018 void config_motor();
00019 void read_color();
00020 void controllo_motori();
00021 void check_color();
00022 
00023 
00024 TCS3200 color(D3,D8,D12,D13,D14);
00025 DigitalIn ir1 (D10); //destra
00026 DigitalIn ir2 (D11); //sinistra
00027 
00028 /*Variabili per leggere i colori.*/
00029 
00030 long red;
00031 long green;
00032 long blue;
00033 
00034 /*Variabili per abilitare i motori sono dopo una prima lettura del sensore di colore,poichè inizialmente da sempre nessun colore come comando*/
00035 int start=0;
00036 
00037 
00038 /*Definizione thread per lettura colori*/
00039 Thread thread;
00040 
00041 
00042 /* Main ----------------------------------------------------------------------*/
00043 Serial pc(USBTX,USBRX);
00044 
00045 void read_color()
00046 {
00047 
00048     while(true) {
00049        check_color();
00050        start=1;
00051         
00052         Thread::wait(100);
00053     }
00054 }
00055 
00056 int main()
00057 {
00058 
00059     config_motor();
00060 
00061     //Set the scaling factor 
00062     color.SetMode(TCS3200::SCALE_100);
00063 
00064 
00065     thread.start(read_color);
00066            
00067     while(true){
00068         
00069         if(start==1) {
00070             
00071            controllo_motori(); //se il sensore ha effettuato una prima lettura allora avvio il controllo motori
00072             
00073             }
00074             
00075         
00076         else if(start==0) {
00077             motor->set_speed(0,0);                          
00078             motor->set_speed(1,0);
00079             motor->run(1, BDCMotor::BWD);                   
00080             motor->run(0, BDCMotor::BWD);
00081             }   
00082     }
00083 }
00084 
00085 //Elenco Funzioni
00086 void config_motor(){
00087   #if (defined TARGET_NUCLEO_F030R8)||(defined TARGET_NUCLEO_F334R8)
00088   motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A2);
00089   #elif (defined TARGET_NUCLEO_L152RE)
00090   motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A3);
00091   #elif (defined TARGET_NUCLEO_F429ZI)
00092   motor = new STSpin240_250(D2, D9, D6, D7, D5, D3, A0);
00093   #else
00094   motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A0);
00095   #endif
00096   if (motor->init(&init) != COMPONENT_OK) exit(EXIT_FAILURE);
00097 
00098   motor->set_dual_full_bridge_config(1);
00099   motor->set_ref_pwm_freq(0, 15000); 
00100   motor->set_ref_pwm_dc(0, 60); 
00101   motor->set_bridge_input_pwm_freq(0,10000); 
00102   motor->set_bridge_input_pwm_freq(1,10000); 
00103 }
00104 
00105 void check_color(){    
00106      red = color.ReadRed();
00107      blue=color.ReadBlue();
00108      green = color.ReadGreen();  
00109 }
00110 
00111 void controllo_motori(){
00112 
00113         if(red<green && red<blue) {
00114             
00115         motor->set_speed(0,0);                          
00116         motor->set_speed(1,0);
00117         motor->run(1, BDCMotor::FWD);                   
00118         motor->run(0, BDCMotor::FWD);
00119             
00120         }
00121         
00122         else {
00123         motor->set_speed(0,40);                          
00124         motor->set_speed(1,40);
00125         motor->run(1, BDCMotor::FWD);                   
00126         motor->run(0, BDCMotor::FWD); 
00127 }
00128     
00129     
00130         if (ir1 && !ir2) {
00131             motor->set_speed(0,40);
00132             motor->set_speed(1,100); 
00133             motor->run(0, BDCMotor::BWD);
00134             motor->run(1, BDCMotor::FWD); 
00135         }
00136 
00137         if (!ir1 && ir2) {
00138             motor->set_speed(0,100);
00139             motor->set_speed(1,40); 
00140             motor->run(0, BDCMotor::FWD);
00141             motor->run(1, BDCMotor::BWD); 
00142         }
00143 }    
00144         
00145