Bologna
Dependencies: TCS3200 X_NUCLEO_IHM12A1 mbed-rtos mbed
Fork of SchedamotoriIHM12A1_copy by
main.cpp
- Committer:
- gandhi1
- Date:
- 2018-04-26
- Revision:
- 7:e14976007f66
- Parent:
- 6:b5d006c1eb59
File content as of revision 7:e14976007f66:
#include "mbed.h" #include "rtos.h" #include "TCS3200.h" #include "STSpin240_250.h" STSpin240_250_init_t init = { 20000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz */ 20000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz */ 20000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz */ 50, /* Duty cycle of PWM used for Ref pin (from 0 to 100) */ TRUE /* Dual Bridge configuration (FALSE for mono, TRUE for dual brush dc) */ }; STSpin240_250 *motor; /*FUNZIONI:.*/ void config_motor(); void read_color(); void controllo_motori(); void check_color(); TCS3200 color(D3,D8,D12,D13,D14); DigitalIn ir1 (D10); //destra DigitalIn ir2 (D11); //sinistra /*Variabili per leggere i colori.*/ long red; long green; long blue; /*Variabili per abilitare i motori sono dopo una prima lettura del sensore di colore,poichè inizialmente da sempre nessun colore come comando*/ int start=0; /*Definizione thread per lettura colori*/ Thread thread; /* Main ----------------------------------------------------------------------*/ Serial pc(USBTX,USBRX); void read_color() { while(true) { check_color(); start=1; Thread::wait(100); } } int main() { config_motor(); //Set the scaling factor color.SetMode(TCS3200::SCALE_100); thread.start(read_color); while(true){ if(start==1) { controllo_motori(); //se il sensore ha effettuato una prima lettura allora avvio il controllo motori } else if(start==0) { motor->set_speed(0,0); motor->set_speed(1,0); motor->run(1, BDCMotor::BWD); motor->run(0, BDCMotor::BWD); } } } //Elenco Funzioni void config_motor(){ #if (defined TARGET_NUCLEO_F030R8)||(defined TARGET_NUCLEO_F334R8) motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A2); #elif (defined TARGET_NUCLEO_L152RE) motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A3); #elif (defined TARGET_NUCLEO_F429ZI) motor = new STSpin240_250(D2, D9, D6, D7, D5, D3, A0); #else motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A0); #endif if (motor->init(&init) != COMPONENT_OK) exit(EXIT_FAILURE); motor->set_dual_full_bridge_config(1); motor->set_ref_pwm_freq(0, 15000); motor->set_ref_pwm_dc(0, 60); motor->set_bridge_input_pwm_freq(0,10000); motor->set_bridge_input_pwm_freq(1,10000); } void check_color(){ red = color.ReadRed(); blue=color.ReadBlue(); green = color.ReadGreen(); } void controllo_motori(){ if(red<green && red<blue) { motor->set_speed(0,0); motor->set_speed(1,0); motor->run(1, BDCMotor::FWD); motor->run(0, BDCMotor::FWD); } else { motor->set_speed(0,40); motor->set_speed(1,40); motor->run(1, BDCMotor::FWD); motor->run(0, BDCMotor::FWD); } if (ir1 && !ir2) { motor->set_speed(0,40); motor->set_speed(1,100); motor->run(0, BDCMotor::BWD); motor->run(1, BDCMotor::FWD); } if (!ir1 && ir2) { motor->set_speed(0,100); motor->set_speed(1,40); motor->run(0, BDCMotor::FWD); motor->run(1, BDCMotor::BWD); } }