self balancing car unfinished
Dependencies: X_NUCLEO_IHM02A1 X_NUCLEO_IKS01A2 mbed-rtos mbed
main.cpp
- Committer:
- yong304
- Date:
- 2018-04-04
- Revision:
- 0:a1ef0a01d09f
File content as of revision 0:a1ef0a01d09f:
/** ****************************************************************************** * @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); } }