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
Revision 0:6bde2b4e342e, committed 2017-04-04
- Comitter:
- Giuliove
- Date:
- Tue Apr 04 09:25:16 2017 +0000
- Commit message:
- Inseguimento di una traiettoria dritta mediante misure fornite dal giroscopio
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X-NUCLEO-IHM12A1.lib Tue Apr 04 09:25:16 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/X_NUCLEO_IHM12A1/#232e0c730f59
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X_NUCLEO_IKS01A2.lib Tue Apr 04 09:25:16 2017 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A2/#63b2b4c21092
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 04 09:25:16 2017 +0000
@@ -0,0 +1,205 @@
+ /**
+ ******************************************************************************
+ * @file main.cpp
+ * @author Giulio Vespoli, Unina Fablab
+ * @version 1.0
+ * @date 04-April-2017
+ * @brief Simple Example application for using the X_NUCLEO_IKS01A1
+ * MEMS Inertial & Environmental Sensor Nucleo expansion board.
+ ******************************************************************************
+*/
+
+
+/* Includes */
+#include "mbed.h"
+#include "rtos.h"
+#include "x_nucleo_iks01a2.h"
+
+#define RAD_TO_DEG 57.295779513082320876798154814105f
+
+
+
+/* Instantiate the expansion board */
+static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D10, D11); //d4 diventa d11 e d5 diventa d10
+
+static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
+static LSM303AGR_ACC_Sensor *accelerometer = mems_expansion_board->accelerometer;
+
+
+int32_t mdps[3];
+int16_t mdpsRaw[3];
+int32_t acc[3];
+
+
+bool useCalibrate = false;
+float dpsPerDigit = .07f; //70 mdps diventeranno 0.07dps
+float gyrosensitivity;
+
+
+float offset=0;
+float correct_speed=0;
+float acquired_speed=0;
+float yaw = 0;
+
+
+
+void readRaw()
+{
+ acc_gyro->Get_G_AxesRaw(mdpsRaw);
+ acquired_speed = mdpsRaw[2]; //acquisisco velocità angolare dal giroscopio
+}
+
+
+
+float readNormalize()
+{
+ readRaw();
+ correct_speed = (acquired_speed - offset) * dpsPerDigit; //tolgo al valore acquisito l'offset
+ return correct_speed;
+}
+
+
+
+void calibrate(int samples)
+{
+ printf("CALIBRATING\r\n");
+
+ float sumZ = 0;
+ for (uint8_t i = 0; i < samples; ++i)
+ {
+ acc_gyro->Get_G_AxesRaw(mdpsRaw); //Acquisisco dal giroscopio
+ printf("SAMPLE - LSM6DSL [gyro/mdps]: %6ld, %6ld,\r\n", i, mdpsRaw[2]); //Stampo il numero del campione acquisito e la velocità angolare
+ sumZ += mdpsRaw[2]; //Faccio la somma delle velocità angolari acquisite
+ wait(0.1);
+ }
+
+ offset = sumZ / samples; //calcolo l'offset come valor medio di 100 acquisizioni
+
+ printf("CALIBRATED\r\n");
+ printf("\r\n");
+}
+
+
+
+void calcRotation2(void const *args)
+{
+
+ Timer t;
+ double dt=0; //in dt memorizzerò l'intervallo di tempo tra due acquisizioni
+
+ while(1)
+ {
+ t.stop();
+ dt= t.read();
+
+ correct_speed = readNormalize();
+ t.reset();
+ t.start();
+
+ // Calcolo lo yaw
+ yaw = yaw + correct_speed * dt; //lo spostamento complessivo al passo k sarà uguale allo spostamento complessivo all'istante k-1 + la nuova rotazione
+ }
+}
+
+
+
+
+
+/* Component specific header files. */
+#include "stspin240_250_class.h"
+STSPIN240_250 *motor;
+
+
+int main()
+{
+ uint8_t id;
+ acc_gyro->Enable_X(); //ABILITO ACCELLEROMETRO
+ acc_gyro->Enable_G(); //ABILITO GIROSCOPIO
+
+ accelerometer->Enable(); //ABILITO UN ALTRO ACCELLEROMETRO
+
+ printf("\r\n--- Starting new run ---\r\n");
+
+ wait(1);
+
+ acc_gyro->ReadID(&id); //RICAVO L'INDIRIZZO DEL GIROSCOPIO E ACCELLEROMETRO
+ printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+
+ accelerometer->ReadID(&id); //RICAVO L'INDIRIZZO DELL'ACCELLEROMETRO
+ printf("LSM303AGR accelerometer = 0x%X\r\n", id);
+
+
+ acc_gyro->Get_G_Sensitivity(&gyrosensitivity); //RICAVO LA SENSIBILITA' DEL GIRO E ACC, 70
+ printf("Sensitivity mV/o/ms: %f\r\n", gyrosensitivity);
+
+ float fullScale;
+ acc_gyro->Get_G_FS(&fullScale);
+ printf("Full Scale: %f\r\n", fullScale);
+
+ float gODR;
+ acc_gyro->Get_G_ODR(&gODR);
+ printf("ODR: %f\r\n", gODR);
+
+ calibrate(100); //Uso 100 campioni per calcolare l'offset
+
+ Thread rotTrigger(calcRotation2); //Chiamo la funzione calcRotation2 che aggiorna il valore dello yaw
+
+
+
+ /* Initializing Motor Control Component. */
+ motor = new STSPIN240_250(D2, D9, D6, D7, D5, D4, A0 ); //Chiamo il costruttore per inizializzare l'oggetto motor
+
+ /* Set dual bridge enabled as two motors are used*/
+ motor->SetDualFullBridgeConfig(1);
+
+ /* Set PWM Frequency of Ref to 20000 Hz */
+ motor->SetRefPwmFreq(0, 20000);
+ motor->SetRefPwmFreq(1, 20000);
+
+ /* Set PWM duty cycle of Ref to 100% */
+ motor->SetRefPwmDc(0, 100);
+
+ motor->SetRefPwmDc(1, 100);
+
+ /* Set PWM Frequency of bridge A inputs to 20000 Hz */
+ motor->SetBridgeInputPwmFreq(0,20000);
+
+ /* Set PWM Frequency of bridge B inputs to 20000 Hz */
+ motor->SetBridgeInputPwmFreq(1,20000);
+
+
+
+ int speed_sx=35;
+ int speed_dx=35;
+
+
+ motor->SetSpeed(0,speed_sx);
+ motor->SetSpeed(1,speed_dx);
+ motor->Run(1, BDCMotor::FWD);
+ motor->Run(0, BDCMotor::FWD);
+
+ while(1)
+ {
+ if(yaw>0.0001f) //90° corrisponde ad uno yaw di 0.08, quindi 0.0001 corrisponde ad un decimo di grado (0.1°)
+ {
+ speed_sx=45;
+ speed_dx=35;
+ }
+
+ if(yaw<-0.0001f)
+ {
+ speed_dx=45;
+ speed_sx=35;
+ }
+
+ printf( "yaw = %2.4f \r\n", yaw);
+ motor->SetSpeed(0,speed_sx);
+ motor->SetSpeed(1,speed_dx);
+ printf( "VELOCITA SX = %6ld\r\n", speed_sx);
+ printf( "VELOCITA DX= %6ld\r\n", speed_dx);
+ printf("\r\n");
+
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Tue Apr 04 09:25:16 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#12552ef4e980
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Apr 04 09:25:16 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/d75b3fe1f5cb \ No newline at end of file