Liceo Statale Gandhi / Mbed 2 deprecated Bologna

Bologna

Dependencies:   TCS3200 X_NUCLEO_IHM12A1 mbed-rtos mbed

Fork of SchedamotoriIHM12A1_copy by Duckiebot

main.cpp@7:e14976007f66, 2018-04-26 (annotated)

Committer:
gandhi1
Date:
Thu Apr 26 11:00:39 2018 +0000
Revision:
7:e14976007f66
Parent:
6:b5d006c1eb59 
Bologna

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Manu_L 0:773e2a2be16f 1 #include "mbed.h"
gandhi1 7:e14976007f66 2 #include "rtos.h"
gandhi1 7:e14976007f66 3 #include "TCS3200.h"
gandhi1 7:e14976007f66 4 #include "STSpin240_250.h"
gandhi1 7:e14976007f66 5
gandhi1 7:e14976007f66 6 STSpin240_250_init_t init =
Manu_L 0:773e2a2be16f 7 {
gandhi1 7:e14976007f66 8 20000, /* Frequency of PWM of Input Bridge A in Hz up to 100000Hz */
gandhi1 7:e14976007f66 9 20000, /* Frequency of PWM of Input Bridge B in Hz up to 100000Hz */
gandhi1 7:e14976007f66 10 20000, /* Frequency of PWM used for Ref pin in Hz up to 100000Hz */
gandhi1 7:e14976007f66 11 50, /* Duty cycle of PWM used for Ref pin (from 0 to 100) */
gandhi1 7:e14976007f66 12 TRUE /* Dual Bridge configuration (FALSE for mono, TRUE for dual brush dc) */
Manu_L 0:773e2a2be16f 13 };
gandhi1 7:e14976007f66 14
gandhi1 7:e14976007f66 15 STSpin240_250 *motor;
gandhi1 7:e14976007f66 16
gandhi1 7:e14976007f66 17 /*FUNZIONI:.*/
gandhi1 7:e14976007f66 18 void config_motor();
gandhi1 7:e14976007f66 19 void read_color();
gandhi1 7:e14976007f66 20 void controllo_motori();
gandhi1 7:e14976007f66 21 void check_color();
gandhi1 7:e14976007f66 22
gandhi1 7:e14976007f66 23
gandhi1 7:e14976007f66 24 TCS3200 color(D3,D8,D12,D13,D14);
gandhi1 7:e14976007f66 25 DigitalIn ir1 (D10); //destra
gandhi1 7:e14976007f66 26 DigitalIn ir2 (D11); //sinistra
gandhi1 7:e14976007f66 27
gandhi1 7:e14976007f66 28 /*Variabili per leggere i colori.*/
gandhi1 7:e14976007f66 29
gandhi1 7:e14976007f66 30 long red;
gandhi1 7:e14976007f66 31 long green;
gandhi1 7:e14976007f66 32 long blue;
gandhi1 7:e14976007f66 33
gandhi1 7:e14976007f66 34 /*Variabili per abilitare i motori sono dopo una prima lettura del sensore di colore,poichè inizialmente da sempre nessun colore come comando*/
gandhi1 7:e14976007f66 35 int start=0;
gandhi1 7:e14976007f66 36
gandhi1 7:e14976007f66 37
gandhi1 7:e14976007f66 38 /*Definizione thread per lettura colori*/
gandhi1 7:e14976007f66 39 Thread thread;
gandhi1 7:e14976007f66 40
gandhi1 7:e14976007f66 41
gandhi1 7:e14976007f66 42 /* Main ----------------------------------------------------------------------*/
gandhi1 7:e14976007f66 43 Serial pc(USBTX,USBRX);
gandhi1 7:e14976007f66 44
gandhi1 7:e14976007f66 45 void read_color()
gandhi1 7:e14976007f66 46 {
gandhi1 7:e14976007f66 47
gandhi1 7:e14976007f66 48 while(true) {
gandhi1 7:e14976007f66 49 check_color();
gandhi1 7:e14976007f66 50 start=1;
gandhi1 7:e14976007f66 51
gandhi1 7:e14976007f66 52 Thread::wait(100);
gandhi1 7:e14976007f66 53 }
gandhi1 7:e14976007f66 54 }
Manu_L 0:773e2a2be16f 55
Manu_L 0:773e2a2be16f 56 int main()
Manu_L 0:773e2a2be16f 57 {
gandhi1 7:e14976007f66 58
gandhi1 7:e14976007f66 59 config_motor();
gandhi1 7:e14976007f66 60
gandhi1 7:e14976007f66 61 //Set the scaling factor
gandhi1 7:e14976007f66 62 color.SetMode(TCS3200::SCALE_100);
gandhi1 7:e14976007f66 63
gandhi1 6:b5d006c1eb59 64
gandhi1 7:e14976007f66 65 thread.start(read_color);
gandhi1 7:e14976007f66 66
gandhi1 7:e14976007f66 67 while(true){
gandhi1 7:e14976007f66 68
gandhi1 7:e14976007f66 69 if(start==1) {
gandhi1 7:e14976007f66 70
gandhi1 7:e14976007f66 71 controllo_motori(); //se il sensore ha effettuato una prima lettura allora avvio il controllo motori
gandhi1 7:e14976007f66 72
gandhi1 7:e14976007f66 73 }
gandhi1 7:e14976007f66 74
gandhi1 7:e14976007f66 75
gandhi1 7:e14976007f66 76 else if(start==0) {
gandhi1 7:e14976007f66 77 motor->set_speed(0,0);
gandhi1 7:e14976007f66 78 motor->set_speed(1,0);
gandhi1 7:e14976007f66 79 motor->run(1, BDCMotor::BWD);
gandhi1 7:e14976007f66 80 motor->run(0, BDCMotor::BWD);
gandhi1 7:e14976007f66 81 }
Manu_L 0:773e2a2be16f 82 }
Manu_L 0:773e2a2be16f 83 }
DonatoSt 5:21a4085d2057 84
gandhi1 7:e14976007f66 85 //Elenco Funzioni
gandhi1 7:e14976007f66 86 void config_motor(){
gandhi1 7:e14976007f66 87 #if (defined TARGET_NUCLEO_F030R8)||(defined TARGET_NUCLEO_F334R8)
gandhi1 7:e14976007f66 88 motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A2);
gandhi1 7:e14976007f66 89 #elif (defined TARGET_NUCLEO_L152RE)
gandhi1 7:e14976007f66 90 motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A3);
gandhi1 7:e14976007f66 91 #elif (defined TARGET_NUCLEO_F429ZI)
gandhi1 7:e14976007f66 92 motor = new STSpin240_250(D2, D9, D6, D7, D5, D3, A0);
gandhi1 7:e14976007f66 93 #else
gandhi1 7:e14976007f66 94 motor = new STSpin240_250(D2, D9, D6, D7, D5, D4, A0);
gandhi1 7:e14976007f66 95 #endif
gandhi1 7:e14976007f66 96 if (motor->init(&init) != COMPONENT_OK) exit(EXIT_FAILURE);
gandhi1 7:e14976007f66 97
gandhi1 7:e14976007f66 98 motor->set_dual_full_bridge_config(1);
gandhi1 7:e14976007f66 99 motor->set_ref_pwm_freq(0, 15000);
gandhi1 7:e14976007f66 100 motor->set_ref_pwm_dc(0, 60);
gandhi1 7:e14976007f66 101 motor->set_bridge_input_pwm_freq(0,10000);
gandhi1 7:e14976007f66 102 motor->set_bridge_input_pwm_freq(1,10000);
gandhi1 7:e14976007f66 103 }
gandhi1 7:e14976007f66 104
gandhi1 7:e14976007f66 105 void check_color(){
gandhi1 7:e14976007f66 106 red = color.ReadRed();
gandhi1 7:e14976007f66 107 blue=color.ReadBlue();
gandhi1 7:e14976007f66 108 green = color.ReadGreen();
gandhi1 7:e14976007f66 109 }
gandhi1 7:e14976007f66 110
gandhi1 7:e14976007f66 111 void controllo_motori(){
gandhi1 7:e14976007f66 112
gandhi1 7:e14976007f66 113 if(red<green && red<blue) {
gandhi1 7:e14976007f66 114
gandhi1 7:e14976007f66 115 motor->set_speed(0,0);
gandhi1 7:e14976007f66 116 motor->set_speed(1,0);
gandhi1 7:e14976007f66 117 motor->run(1, BDCMotor::FWD);
gandhi1 7:e14976007f66 118 motor->run(0, BDCMotor::FWD);
gandhi1 7:e14976007f66 119
gandhi1 7:e14976007f66 120 }
gandhi1 7:e14976007f66 121
gandhi1 7:e14976007f66 122 else {
gandhi1 7:e14976007f66 123 motor->set_speed(0,40);
gandhi1 7:e14976007f66 124 motor->set_speed(1,40);
gandhi1 7:e14976007f66 125 motor->run(1, BDCMotor::FWD);
gandhi1 7:e14976007f66 126 motor->run(0, BDCMotor::FWD);
gandhi1 7:e14976007f66 127 }
gandhi1 7:e14976007f66 128
gandhi1 7:e14976007f66 129
gandhi1 7:e14976007f66 130 if (ir1 && !ir2) {
gandhi1 7:e14976007f66 131 motor->set_speed(0,40);
gandhi1 7:e14976007f66 132 motor->set_speed(1,100);
gandhi1 7:e14976007f66 133 motor->run(0, BDCMotor::BWD);
gandhi1 7:e14976007f66 134 motor->run(1, BDCMotor::FWD);
gandhi1 7:e14976007f66 135 }
gandhi1 7:e14976007f66 136
gandhi1 7:e14976007f66 137 if (!ir1 && ir2) {
gandhi1 7:e14976007f66 138 motor->set_speed(0,100);
gandhi1 7:e14976007f66 139 motor->set_speed(1,40);
gandhi1 7:e14976007f66 140 motor->run(0, BDCMotor::FWD);
gandhi1 7:e14976007f66 141 motor->run(1, BDCMotor::BWD);
gandhi1 7:e14976007f66 142 }
gandhi1 7:e14976007f66 143 }
gandhi1 7:e14976007f66 144
gandhi1 7:e14976007f66 145