ching-yang Trui
self balancing car unfinished
Dependencies: X_NUCLEO_IHM02A1 X_NUCLEO_IKS01A2 mbed-rtos mbed
Revision 0:a1ef0a01d09f, committed 2018-04-04
- Comitter:
- yong304
- Date:
- Wed Apr 04 08:30:55 2018 +0000
- Commit message:
- self balancing car unfinished
Changed in this revision
diff -r 000000000000 -r a1ef0a01d09f X_NUCLEO_IHM02A1.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X_NUCLEO_IHM02A1.lib Wed Apr 04 08:30:55 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/yong304/code/X_NUCLEO_IHM02A1/#66409b9ed27c
diff -r 000000000000 -r a1ef0a01d09f X_NUCLEO_IKS01A2.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X_NUCLEO_IKS01A2.lib Wed Apr 04 08:30:55 2018 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/ST/code/X_NUCLEO_IKS01A2/#138a7a28bd21
diff -r 000000000000 -r a1ef0a01d09f main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Apr 04 08:30:55 2018 +0000
@@ -0,0 +1,233 @@
+/**
+ ******************************************************************************
+ * @file main.cpp
+ * @author CLab
+ * @version V1.0.0
+ * @date 2-December-2016
+ * @brief Simple Example application for using the X_NUCLEO_IKS01A1
+ * MEMS Inertial & Environmental Sensor Nucleo expansion board.
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ * 3. Neither the name of STMicroelectronics nor the names of its contributors
+ * may be used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ******************************************************************************
+*/
+
+/* Includes */
+#include "DevSPI.h"
+#include "XNucleoIHM02A1.h"
+#include "mbed.h"
+#include "XNucleoIKS01A2.h"
+#include "rtos.h"
+#include "iostream"
+using namespace std;
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+DigitalOut actuatedLED(LED2);
+void move_thread();
+
+
+/* Initialization parameters of the motors connected to the expansion board. */
+//L6470_init_t init[L6470DAISYCHAINSIZE] = {
+L6470_init_t init[2] = {
+ /* First Motor. */
+ {
+ 12.0, /* Motor supply voltage in V. */
+ 200, /* Min number of steps per revolution for the motor. */
+ 1.7, /* Max motor phase voltage in A. */
+ 3.06, /* Max motor phase voltage in V. */
+ 30000.0, /* Motor initial speed [step/s]. */
+ // 5000, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ // 5000, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 2500.0, /* Motor maximum speed [step/s]. */
+ 0.0, /* Motor minimum speed [step/s]. */
+ 602.7, /* Motor full-step speed threshold [step/s]. */
+ 3.06, /* Holding kval [V]. */
+ 3.06, /* Constant speed kval [V]. */
+ 3.06, /* Acceleration starting kval [V]. */
+ 3.06, /* Deceleration starting kval [V]. */
+ 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
+ 392.1569e-6, /* Start slope [s/step]. */
+ 643.1372e-6, /* Acceleration final slope [s/step]. */
+ 643.1372e-6, /* Deceleration final slope [s/step]. */
+ 0, /* Thermal compensation factor (range [0, 15]). */
+ 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
+ 0xFF, /* Alarm conditions enable. */
+ 0x2E88 /* Ic configuration. */
+ },
+
+ /* Second Motor. */
+ {
+ 12.0, /* Motor supply voltage in V. */
+ 200, /* Min number of steps per revolution for the motor. */
+ 1.7, /* Max motor phase voltage in A. */
+ 3.06, /* Max motor phase voltage in V. */
+ 300.0, /* Motor initial speed [step/s]. */
+ // 10000, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
+ // 10000, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
+ 2300.0, /* Motor maximum speed [step/s]. */
+ 0.0, /* Motor minimum speed [step/s]. */
+ 602.7, /* Motor full-step speed threshold [step/s]. */
+ 3.06, /* Holding kval [V]. */
+ 3.06, /* Constant speed kval [V]. */
+ 3.06, /* Acceleration starting kval [V]. */
+ 3.06, /* Deceleration starting kval [V]. */
+ 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
+ 392.1569e-6, /* Start slope [s/step]. */
+ 643.1372e-6, /* Acceleration final slope [s/step]. */
+ 643.1372e-6, /* Deceleration final slope [s/step]. */
+ 0, /* Thermal compensation factor (range [0, 15]). */
+ 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
+ 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
+ StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
+ 0xFF, /* Alarm conditions enable. */
+ 0x2E88 /* Ic configuration. */
+ }
+};
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+#define sampleTime 0.003
+#define targetAngle 6
+#define Kp 15
+#define Ki 0
+#define Kd 0
+#define pi 3.1415926
+
+ Timer timer;
+ int begin, end;
+ Thread thread;
+// int32_t accpaxes[3];
+// double accpdaxes[3];
+ int32_t accaxes[3];
+ double accdaxes[3];
+ int32_t gyroaxes[3];
+ double gyrodaxes[3];
+ int32_t motorspeed;
+ double currentAngle;
+ double accpAngle;
+ double accAngle;
+ double errSum;
+ double err;
+ double prevAngle;
+ double theta;
+ double gyroAngle;
+
+
+#ifdef TARGET_STM32F429
+ DevSPI dev_spi(D11, D12, D13);
+#else
+ DevSPI dev_spi(D11, D12, D3);
+#endif
+ XNucleoIHM02A1 *x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
+/* Instantiate the expansion board */
+static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
+/* Retrieve the composing elements of the expansion board */
+static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
+//static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
+/* Simple main function */
+
+
+
+int main() {
+ uint8_t id;
+ /* Enable sensors */
+ //accelerometer->enable();
+ acc_gyro->enable_x();
+ //acc_gyro->enable_g();
+ //printf("\r\n--- Starting new run ---\r\n");
+
+ //accelerometer->read_id(&id);
+ //printf("LSM303AGR accelerometer = 0x%X\r\n", id);
+ acc_gyro->read_id(&id);
+ printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
+
+ thread.start(move_thread);
+}
+
+
+void move_thread() {
+
+ while (true) {
+
+ L6470 **motors = x_nucleo_ihm02a1->get_components();
+ timer.start();
+ actuatedLED=0;
+ begin = timer.read_us();
+ //accelerometer->get_x_axes(accpaxes);
+ //accpdaxes[0] = accpaxes[0] +40;
+ //accpdaxes[2] = accpaxes[2] +40;
+ //theta = accpdaxes[0]/accpdaxes[2];
+ acc_gyro->get_x_axes(accaxes);
+ accdaxes[0] = accaxes[0];
+ accdaxes[2] = accaxes[2];
+ theta = accdaxes[0]/accdaxes[2];
+ //accpAngle = atan(theta)*180/pi;
+ accAngle = atan(theta)*180/pi;
+ //acc_gyro->get_g_axes(gyroaxes);
+ //gyrodaxes[1] = gyroaxes[1];
+ //gyroAngle = (gyrodaxes[1])*sampleTime*0.001;
+ //currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
+ currentAngle = accAngle;
+ err = currentAngle - targetAngle;
+ errSum = errSum + err;
+ motorspeed = Kp*err + Ki*errSum*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
+
+ // if (motorspeed > 0){
+ motors[0]->run(StepperMotor::FWD, 1000);
+ motors[1]->run(StepperMotor::BWD, 3000);
+ // }else if(motorspeed <= 0) {
+ // motors[0]->run(StepperMotor::BWD, -motorspeed);
+ // motors[1]->run(StepperMotor::FWD, -motorspeed);
+ // }
+ uint32_t r,r2;
+ // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
+ r = motors[0]->get_acceleration();
+ r2 = motors[1]->get_acceleration();
+ // }
+ // uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
+
+ printf(" Speed: M1 %d, M2 %d.\r\n", r, r2);
+ //x_nucleo_ihm02a1->perform_prepared_actions();
+ //prevAngle = currentAngle;
+
+ // printf("LSM303AGR [acc/mg]: %6ld, %6ld, %6ld\r\n", accpaxes[0], accpaxes[1], accpaxes[2]);
+ // printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", gyroaxes[0], gyroaxes[1], gyroaxes[2]);
+ // printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", accaxes[0]-22, accaxes[1]+50, accaxes[2]-45);
+ // printf("accAngle = %6lf\r\n", accAngle );
+ // printf("accpAngle = %6lf\r\n", accpAngle );
+ // printf("theta = %6lf\r\n", theta );
+ // printf("currentAngle = %6lf\r\n", currentAngle );
+ // printf("gyroAngle = %6lf\r\n", gyroAngle );
+ Thread::wait(2);
+ end = timer.read_us();
+ actuatedLED=1;
+ // printf("Toggle the led takes %d us \r\n", end - begin);
+
+ }
+}
diff -r 000000000000 -r a1ef0a01d09f mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Wed Apr 04 08:30:55 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
diff -r 000000000000 -r a1ef0a01d09f mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Apr 04 08:30:55 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90 \ No newline at end of file