DuckietownHS
Ottimi risultati, provare sia con alimentazione di 3.3V che con quella di 5V per scegliere l'andamento desiderato
Dependencies: X_NUCLEO_IHM12A1 X_NUCLEO_IKS01A2 mbed-rtos mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file main.cpp 00004 * @author Giulio Vespoli, Unina Fablab 00005 * @version 1.0 00006 * @date 04-April-2017 00007 * @brief Simple Example application for using the X_NUCLEO_IKS01A1 00008 * MEMS Inertial & Environmental Sensor Nucleo expansion board. 00009 ****************************************************************************** 00010 */ 00011 00012 00013 /* Includes */ 00014 #include "mbed.h" 00015 #include "rtos.h" 00016 #include "x_nucleo_iks01a2.h" 00017 00018 #define RAD_TO_DEG 57.295779513082320876798154814105f 00019 00020 00021 00022 /* Instantiate the expansion board */ 00023 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11); //d4 diventa d11 e d5 diventa d10 00024 00025 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro; 00026 static LSM303AGR_ACC_Sensor *accelerometer = mems_expansion_board->accelerometer; 00027 00028 00029 int32_t mdps[3]; 00030 int16_t mdpsRaw[3]; 00031 int32_t acc[3]; 00032 00033 00034 bool useCalibrate = false; 00035 float dpsPerDigit = .07f; //70 mdps diventeranno 0.07dps 00036 float gyrosensitivity; 00037 00038 00039 float offset=0; 00040 float correct_speed=0; 00041 float acquired_speed=0; 00042 float yaw = 0; 00043 00044 00045 00046 void readRaw() 00047 { 00048 acc_gyro->Get_G_AxesRaw(mdpsRaw); 00049 acquired_speed = mdpsRaw[2]; //acquisisco velocità angolare dal giroscopio 00050 } 00051 00052 00053 00054 float readNormalize() 00055 { 00056 readRaw(); 00057 correct_speed = (acquired_speed - offset) * dpsPerDigit; //tolgo al valore acquisito l'offset 00058 return correct_speed; 00059 } 00060 00061 00062 00063 void calibrate(int samples) 00064 { 00065 printf("CALIBRATING\r\n"); 00066 00067 float sumZ = 0; 00068 for (uint8_t i = 0; i < samples; ++i) 00069 { 00070 acc_gyro->Get_G_AxesRaw(mdpsRaw); //Acquisisco dal giroscopio 00071 printf("SAMPLE - LSM6DSL [gyro/mdps]: %6ld, %6ld,\r\n", i, mdpsRaw[2]); //Stampo il numero del campione acquisito e la velocità angolare 00072 sumZ += mdpsRaw[2]; //Faccio la somma delle velocità angolari acquisite 00073 wait(0.1); 00074 } 00075 00076 offset = sumZ / samples; //calcolo l'offset come valor medio di 100 acquisizioni 00077 00078 printf("CALIBRATED\r\n"); 00079 printf("\r\n"); 00080 } 00081 00082 00083 00084 void calcRotation2(void const *args) 00085 { 00086 00087 Timer t; 00088 double dt=0; //in dt memorizzerò l'intervallo di tempo tra due acquisizioni 00089 00090 while(1) 00091 { 00092 t.stop(); 00093 dt= t.read(); 00094 00095 correct_speed = readNormalize(); 00096 t.reset(); 00097 t.start(); 00098 00099 // Calcolo lo yaw 00100 yaw = yaw + correct_speed * dt; //lo spostamento complessivo al passo k sarà uguale allo spostamento complessivo all'istante k-1 + la nuova rotazione 00101 } 00102 } 00103 00104 00105 00106 00107 00108 /* Component specific header files. */ 00109 #include "stspin240_250_class.h" 00110 STSPIN240_250 *motor; 00111 00112 00113 int main() 00114 { 00115 uint8_t id; 00116 acc_gyro->Enable_X(); //ABILITO ACCELLEROMETRO 00117 acc_gyro->Enable_G(); //ABILITO GIROSCOPIO 00118 00119 accelerometer->Enable(); //ABILITO UN ALTRO ACCELLEROMETRO 00120 00121 printf("\r\n--- Starting new run ---\r\n"); 00122 00123 wait(1); 00124 00125 acc_gyro->ReadID(&id); //RICAVO L'INDIRIZZO DEL GIROSCOPIO E ACCELLEROMETRO 00126 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id); 00127 00128 accelerometer->ReadID(&id); //RICAVO L'INDIRIZZO DELL'ACCELLEROMETRO 00129 printf("LSM303AGR accelerometer = 0x%X\r\n", id); 00130 00131 00132 acc_gyro->Get_G_Sensitivity(&gyrosensitivity); //RICAVO LA SENSIBILITA' DEL GIRO E ACC, 70 00133 printf("Sensitivity mV/o/ms: %f\r\n", gyrosensitivity); 00134 00135 float fullScale; 00136 acc_gyro->Get_G_FS(&fullScale); 00137 printf("Full Scale: %f\r\n", fullScale); 00138 00139 float gODR; 00140 acc_gyro->Get_G_ODR(&gODR); 00141 printf("ODR: %f\r\n", gODR); 00142 00143 calibrate(100); //Uso 100 campioni per calcolare l'offset 00144 00145 Thread rotTrigger(calcRotation2); //Chiamo la funzione calcRotation2 che aggiorna il valore dello yaw 00146 00147 00148 00149 /* Initializing Motor Control Component. */ 00150 motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 ); //Chiamo il costruttore per inizializzare l'oggetto motor 00151 00152 /* Set dual bridge enabled as two motors are used*/ 00153 motor->SetDualFullBridgeConfig(1); 00154 00155 /* Set PWM Frequency of Ref to 20000 Hz */ 00156 motor->SetRefPwmFreq(0, 20000); 00157 motor->SetRefPwmFreq(1, 20000); 00158 00159 /* Set PWM duty cycle of Ref to 100% */ 00160 motor->SetRefPwmDc(0, 100); 00161 00162 motor->SetRefPwmDc(1, 100); 00163 00164 /* Set PWM Frequency of bridge A inputs to 20000 Hz */ 00165 motor->SetBridgeInputPwmFreq(0,20000); 00166 00167 /* Set PWM Frequency of bridge B inputs to 20000 Hz */ 00168 motor->SetBridgeInputPwmFreq(1,20000); 00169 00170 00171 00172 int speed_sx=35; 00173 int speed_dx=35; 00174 00175 00176 motor->SetSpeed(0,speed_sx); 00177 motor->SetSpeed(1,speed_dx); 00178 motor->Run(1, BDCMotor::FWD); 00179 motor->Run(0, BDCMotor::FWD); 00180 00181 while(1) 00182 { 00183 if(yaw>0.0001f) //90° corrisponde ad uno yaw di 0.08, quindi 0.0001 corrisponde ad un decimo di grado (0.1°) 00184 { 00185 speed_sx=45; 00186 speed_dx=35; 00187 } 00188 00189 if(yaw<-0.0001f) 00190 { 00191 speed_dx=45; 00192 speed_sx=35; 00193 } 00194 00195 printf( "yaw = %2.4f \r\n", yaw); 00196 motor->SetSpeed(0,speed_sx); 00197 motor->SetSpeed(1,speed_dx); 00198 printf( "VELOCITA SX = %6ld\r\n", speed_sx); 00199 printf( "VELOCITA DX= %6ld\r\n", speed_dx); 00200 printf("\r\n"); 00201 00202 } 00203 } 00204 00205
Generated on Thu Jul 28 2022 17:45:45 by
1.7.2