ching-yang Trui / Mbed 2 deprecated selfbalansingcar

self balancing car unfinished

Dependencies:   X_NUCLEO_IHM02A1 X_NUCLEO_IKS01A2 mbed-rtos mbed

main.cpp@0:a1ef0a01d09f, 2018-04-04 (annotated)

Committer:
yong304
Date:
Wed Apr 04 08:30:55 2018 +0000
Revision:
0:a1ef0a01d09f
self balancing car unfinished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yong304 0:a1ef0a01d09f 1 /**
yong304 0:a1ef0a01d09f 2 ******************************************************************************
yong304 0:a1ef0a01d09f 3 * @file main.cpp
yong304 0:a1ef0a01d09f 4 * @author CLab
yong304 0:a1ef0a01d09f 5 * @version V1.0.0
yong304 0:a1ef0a01d09f 6 * @date 2-December-2016
yong304 0:a1ef0a01d09f 7 * @brief Simple Example application for using the X_NUCLEO_IKS01A1
yong304 0:a1ef0a01d09f 8 * MEMS Inertial & Environmental Sensor Nucleo expansion board.
yong304 0:a1ef0a01d09f 9 ******************************************************************************
yong304 0:a1ef0a01d09f 10 * @attention
yong304 0:a1ef0a01d09f 11 *
yong304 0:a1ef0a01d09f 12 * <h2><center>&copy; COPYRIGHT(c) 2016 STMicroelectronics</center></h2>
yong304 0:a1ef0a01d09f 13 *
yong304 0:a1ef0a01d09f 14 * Redistribution and use in source and binary forms, with or without modification,
yong304 0:a1ef0a01d09f 15 * are permitted provided that the following conditions are met:
yong304 0:a1ef0a01d09f 16 * 1. Redistributions of source code must retain the above copyright notice,
yong304 0:a1ef0a01d09f 17 * this list of conditions and the following disclaimer.
yong304 0:a1ef0a01d09f 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
yong304 0:a1ef0a01d09f 19 * this list of conditions and the following disclaimer in the documentation
yong304 0:a1ef0a01d09f 20 * and/or other materials provided with the distribution.
yong304 0:a1ef0a01d09f 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
yong304 0:a1ef0a01d09f 22 * may be used to endorse or promote products derived from this software
yong304 0:a1ef0a01d09f 23 * without specific prior written permission.
yong304 0:a1ef0a01d09f 24 *
yong304 0:a1ef0a01d09f 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
yong304 0:a1ef0a01d09f 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
yong304 0:a1ef0a01d09f 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
yong304 0:a1ef0a01d09f 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
yong304 0:a1ef0a01d09f 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
yong304 0:a1ef0a01d09f 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
yong304 0:a1ef0a01d09f 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
yong304 0:a1ef0a01d09f 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
yong304 0:a1ef0a01d09f 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
yong304 0:a1ef0a01d09f 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
yong304 0:a1ef0a01d09f 35 *
yong304 0:a1ef0a01d09f 36 ******************************************************************************
yong304 0:a1ef0a01d09f 37 */
yong304 0:a1ef0a01d09f 38
yong304 0:a1ef0a01d09f 39 /* Includes */
yong304 0:a1ef0a01d09f 40 #include "DevSPI.h"
yong304 0:a1ef0a01d09f 41 #include "XNucleoIHM02A1.h"
yong304 0:a1ef0a01d09f 42 #include "mbed.h"
yong304 0:a1ef0a01d09f 43 #include "XNucleoIKS01A2.h"
yong304 0:a1ef0a01d09f 44 #include "rtos.h"
yong304 0:a1ef0a01d09f 45 #include "iostream"
yong304 0:a1ef0a01d09f 46 using namespace std;
yong304 0:a1ef0a01d09f 47 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
yong304 0:a1ef0a01d09f 48 DigitalOut actuatedLED(LED2);
yong304 0:a1ef0a01d09f 49 void move_thread();
yong304 0:a1ef0a01d09f 50
yong304 0:a1ef0a01d09f 51
yong304 0:a1ef0a01d09f 52 /* Initialization parameters of the motors connected to the expansion board. */
yong304 0:a1ef0a01d09f 53 //L6470_init_t init[L6470DAISYCHAINSIZE] = {
yong304 0:a1ef0a01d09f 54 L6470_init_t init[2] = {
yong304 0:a1ef0a01d09f 55 /* First Motor. */
yong304 0:a1ef0a01d09f 56 {
yong304 0:a1ef0a01d09f 57 12.0, /* Motor supply voltage in V. */
yong304 0:a1ef0a01d09f 58 200, /* Min number of steps per revolution for the motor. */
yong304 0:a1ef0a01d09f 59 1.7, /* Max motor phase voltage in A. */
yong304 0:a1ef0a01d09f 60 3.06, /* Max motor phase voltage in V. */
yong304 0:a1ef0a01d09f 61 30000.0, /* Motor initial speed [step/s]. */
yong304 0:a1ef0a01d09f 62 // 5000, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
yong304 0:a1ef0a01d09f 63 // 5000, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
yong304 0:a1ef0a01d09f 64 2500.0, /* Motor maximum speed [step/s]. */
yong304 0:a1ef0a01d09f 65 0.0, /* Motor minimum speed [step/s]. */
yong304 0:a1ef0a01d09f 66 602.7, /* Motor full-step speed threshold [step/s]. */
yong304 0:a1ef0a01d09f 67 3.06, /* Holding kval [V]. */
yong304 0:a1ef0a01d09f 68 3.06, /* Constant speed kval [V]. */
yong304 0:a1ef0a01d09f 69 3.06, /* Acceleration starting kval [V]. */
yong304 0:a1ef0a01d09f 70 3.06, /* Deceleration starting kval [V]. */
yong304 0:a1ef0a01d09f 71 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
yong304 0:a1ef0a01d09f 72 392.1569e-6, /* Start slope [s/step]. */
yong304 0:a1ef0a01d09f 73 643.1372e-6, /* Acceleration final slope [s/step]. */
yong304 0:a1ef0a01d09f 74 643.1372e-6, /* Deceleration final slope [s/step]. */
yong304 0:a1ef0a01d09f 75 0, /* Thermal compensation factor (range [0, 15]). */
yong304 0:a1ef0a01d09f 76 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
yong304 0:a1ef0a01d09f 77 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
yong304 0:a1ef0a01d09f 78 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
yong304 0:a1ef0a01d09f 79 0xFF, /* Alarm conditions enable. */
yong304 0:a1ef0a01d09f 80 0x2E88 /* Ic configuration. */
yong304 0:a1ef0a01d09f 81 },
yong304 0:a1ef0a01d09f 82
yong304 0:a1ef0a01d09f 83 /* Second Motor. */
yong304 0:a1ef0a01d09f 84 {
yong304 0:a1ef0a01d09f 85 12.0, /* Motor supply voltage in V. */
yong304 0:a1ef0a01d09f 86 200, /* Min number of steps per revolution for the motor. */
yong304 0:a1ef0a01d09f 87 1.7, /* Max motor phase voltage in A. */
yong304 0:a1ef0a01d09f 88 3.06, /* Max motor phase voltage in V. */
yong304 0:a1ef0a01d09f 89 300.0, /* Motor initial speed [step/s]. */
yong304 0:a1ef0a01d09f 90 // 10000, /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
yong304 0:a1ef0a01d09f 91 // 10000, /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
yong304 0:a1ef0a01d09f 92 2300.0, /* Motor maximum speed [step/s]. */
yong304 0:a1ef0a01d09f 93 0.0, /* Motor minimum speed [step/s]. */
yong304 0:a1ef0a01d09f 94 602.7, /* Motor full-step speed threshold [step/s]. */
yong304 0:a1ef0a01d09f 95 3.06, /* Holding kval [V]. */
yong304 0:a1ef0a01d09f 96 3.06, /* Constant speed kval [V]. */
yong304 0:a1ef0a01d09f 97 3.06, /* Acceleration starting kval [V]. */
yong304 0:a1ef0a01d09f 98 3.06, /* Deceleration starting kval [V]. */
yong304 0:a1ef0a01d09f 99 61.52, /* Intersect speed for bemf compensation curve slope changing [step/s]. */
yong304 0:a1ef0a01d09f 100 392.1569e-6, /* Start slope [s/step]. */
yong304 0:a1ef0a01d09f 101 643.1372e-6, /* Acceleration final slope [s/step]. */
yong304 0:a1ef0a01d09f 102 643.1372e-6, /* Deceleration final slope [s/step]. */
yong304 0:a1ef0a01d09f 103 0, /* Thermal compensation factor (range [0, 15]). */
yong304 0:a1ef0a01d09f 104 3.06 * 1000 * 1.10, /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
yong304 0:a1ef0a01d09f 105 3.06 * 1000 * 1.00, /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
yong304 0:a1ef0a01d09f 106 StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
yong304 0:a1ef0a01d09f 107 0xFF, /* Alarm conditions enable. */
yong304 0:a1ef0a01d09f 108 0x2E88 /* Ic configuration. */
yong304 0:a1ef0a01d09f 109 }
yong304 0:a1ef0a01d09f 110 };
yong304 0:a1ef0a01d09f 111
yong304 0:a1ef0a01d09f 112
yong304 0:a1ef0a01d09f 113 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
yong304 0:a1ef0a01d09f 114
yong304 0:a1ef0a01d09f 115 #define sampleTime 0.003
yong304 0:a1ef0a01d09f 116 #define targetAngle 6
yong304 0:a1ef0a01d09f 117 #define Kp 15
yong304 0:a1ef0a01d09f 118 #define Ki 0
yong304 0:a1ef0a01d09f 119 #define Kd 0
yong304 0:a1ef0a01d09f 120 #define pi 3.1415926
yong304 0:a1ef0a01d09f 121
yong304 0:a1ef0a01d09f 122 Timer timer;
yong304 0:a1ef0a01d09f 123 int begin, end;
yong304 0:a1ef0a01d09f 124 Thread thread;
yong304 0:a1ef0a01d09f 125 // int32_t accpaxes[3];
yong304 0:a1ef0a01d09f 126 // double accpdaxes[3];
yong304 0:a1ef0a01d09f 127 int32_t accaxes[3];
yong304 0:a1ef0a01d09f 128 double accdaxes[3];
yong304 0:a1ef0a01d09f 129 int32_t gyroaxes[3];
yong304 0:a1ef0a01d09f 130 double gyrodaxes[3];
yong304 0:a1ef0a01d09f 131 int32_t motorspeed;
yong304 0:a1ef0a01d09f 132 double currentAngle;
yong304 0:a1ef0a01d09f 133 double accpAngle;
yong304 0:a1ef0a01d09f 134 double accAngle;
yong304 0:a1ef0a01d09f 135 double errSum;
yong304 0:a1ef0a01d09f 136 double err;
yong304 0:a1ef0a01d09f 137 double prevAngle;
yong304 0:a1ef0a01d09f 138 double theta;
yong304 0:a1ef0a01d09f 139 double gyroAngle;
yong304 0:a1ef0a01d09f 140
yong304 0:a1ef0a01d09f 141
yong304 0:a1ef0a01d09f 142 #ifdef TARGET_STM32F429
yong304 0:a1ef0a01d09f 143 DevSPI dev_spi(D11, D12, D13);
yong304 0:a1ef0a01d09f 144 #else
yong304 0:a1ef0a01d09f 145 DevSPI dev_spi(D11, D12, D3);
yong304 0:a1ef0a01d09f 146 #endif
yong304 0:a1ef0a01d09f 147 XNucleoIHM02A1 *x_nucleo_ihm02a1 = new XNucleoIHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
yong304 0:a1ef0a01d09f 148 /* Instantiate the expansion board */
yong304 0:a1ef0a01d09f 149 static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5);
yong304 0:a1ef0a01d09f 150 /* Retrieve the composing elements of the expansion board */
yong304 0:a1ef0a01d09f 151 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
yong304 0:a1ef0a01d09f 152 //static LSM303AGRAccSensor *accelerometer = mems_expansion_board->accelerometer;
yong304 0:a1ef0a01d09f 153 /* Simple main function */
yong304 0:a1ef0a01d09f 154
yong304 0:a1ef0a01d09f 155
yong304 0:a1ef0a01d09f 156
yong304 0:a1ef0a01d09f 157 int main() {
yong304 0:a1ef0a01d09f 158 uint8_t id;
yong304 0:a1ef0a01d09f 159 /* Enable sensors */
yong304 0:a1ef0a01d09f 160 //accelerometer->enable();
yong304 0:a1ef0a01d09f 161 acc_gyro->enable_x();
yong304 0:a1ef0a01d09f 162 //acc_gyro->enable_g();
yong304 0:a1ef0a01d09f 163 //printf("\r\n--- Starting new run ---\r\n");
yong304 0:a1ef0a01d09f 164
yong304 0:a1ef0a01d09f 165 //accelerometer->read_id(&id);
yong304 0:a1ef0a01d09f 166 //printf("LSM303AGR accelerometer = 0x%X\r\n", id);
yong304 0:a1ef0a01d09f 167 acc_gyro->read_id(&id);
yong304 0:a1ef0a01d09f 168 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
yong304 0:a1ef0a01d09f 169
yong304 0:a1ef0a01d09f 170 thread.start(move_thread);
yong304 0:a1ef0a01d09f 171 }
yong304 0:a1ef0a01d09f 172
yong304 0:a1ef0a01d09f 173
yong304 0:a1ef0a01d09f 174 void move_thread() {
yong304 0:a1ef0a01d09f 175
yong304 0:a1ef0a01d09f 176 while (true) {
yong304 0:a1ef0a01d09f 177
yong304 0:a1ef0a01d09f 178 L6470 **motors = x_nucleo_ihm02a1->get_components();
yong304 0:a1ef0a01d09f 179 timer.start();
yong304 0:a1ef0a01d09f 180 actuatedLED=0;
yong304 0:a1ef0a01d09f 181 begin = timer.read_us();
yong304 0:a1ef0a01d09f 182 //accelerometer->get_x_axes(accpaxes);
yong304 0:a1ef0a01d09f 183 //accpdaxes[0] = accpaxes[0] +40;
yong304 0:a1ef0a01d09f 184 //accpdaxes[2] = accpaxes[2] +40;
yong304 0:a1ef0a01d09f 185 //theta = accpdaxes[0]/accpdaxes[2];
yong304 0:a1ef0a01d09f 186 acc_gyro->get_x_axes(accaxes);
yong304 0:a1ef0a01d09f 187 accdaxes[0] = accaxes[0];
yong304 0:a1ef0a01d09f 188 accdaxes[2] = accaxes[2];
yong304 0:a1ef0a01d09f 189 theta = accdaxes[0]/accdaxes[2];
yong304 0:a1ef0a01d09f 190 //accpAngle = atan(theta)*180/pi;
yong304 0:a1ef0a01d09f 191 accAngle = atan(theta)*180/pi;
yong304 0:a1ef0a01d09f 192 //acc_gyro->get_g_axes(gyroaxes);
yong304 0:a1ef0a01d09f 193 //gyrodaxes[1] = gyroaxes[1];
yong304 0:a1ef0a01d09f 194 //gyroAngle = (gyrodaxes[1])*sampleTime*0.001;
yong304 0:a1ef0a01d09f 195 //currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle);
yong304 0:a1ef0a01d09f 196 currentAngle = accAngle;
yong304 0:a1ef0a01d09f 197 err = currentAngle - targetAngle;
yong304 0:a1ef0a01d09f 198 errSum = errSum + err;
yong304 0:a1ef0a01d09f 199 motorspeed = Kp*err + Ki*errSum*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime;
yong304 0:a1ef0a01d09f 200
yong304 0:a1ef0a01d09f 201 // if (motorspeed > 0){
yong304 0:a1ef0a01d09f 202 motors[0]->run(StepperMotor::FWD, 1000);
yong304 0:a1ef0a01d09f 203 motors[1]->run(StepperMotor::BWD, 3000);
yong304 0:a1ef0a01d09f 204 // }else if(motorspeed <= 0) {
yong304 0:a1ef0a01d09f 205 // motors[0]->run(StepperMotor::BWD, -motorspeed);
yong304 0:a1ef0a01d09f 206 // motors[1]->run(StepperMotor::FWD, -motorspeed);
yong304 0:a1ef0a01d09f 207 // }
yong304 0:a1ef0a01d09f 208 uint32_t r,r2;
yong304 0:a1ef0a01d09f 209 // for (int m = 0; m < L6470DAISYCHAINSIZE; m++) {
yong304 0:a1ef0a01d09f 210 r = motors[0]->get_acceleration();
yong304 0:a1ef0a01d09f 211 r2 = motors[1]->get_acceleration();
yong304 0:a1ef0a01d09f 212 // }
yong304 0:a1ef0a01d09f 213 // uint32_t* results = x_nucleo_ihm02a1->perform_prepared_actions();
yong304 0:a1ef0a01d09f 214
yong304 0:a1ef0a01d09f 215 printf(" Speed: M1 %d, M2 %d.\r\n", r, r2);
yong304 0:a1ef0a01d09f 216 //x_nucleo_ihm02a1->perform_prepared_actions();
yong304 0:a1ef0a01d09f 217 //prevAngle = currentAngle;
yong304 0:a1ef0a01d09f 218
yong304 0:a1ef0a01d09f 219 // printf("LSM303AGR [acc/mg]: %6ld, %6ld, %6ld\r\n", accpaxes[0], accpaxes[1], accpaxes[2]);
yong304 0:a1ef0a01d09f 220 // printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", gyroaxes[0], gyroaxes[1], gyroaxes[2]);
yong304 0:a1ef0a01d09f 221 // printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", accaxes[0]-22, accaxes[1]+50, accaxes[2]-45);
yong304 0:a1ef0a01d09f 222 // printf("accAngle = %6lf\r\n", accAngle );
yong304 0:a1ef0a01d09f 223 // printf("accpAngle = %6lf\r\n", accpAngle );
yong304 0:a1ef0a01d09f 224 // printf("theta = %6lf\r\n", theta );
yong304 0:a1ef0a01d09f 225 // printf("currentAngle = %6lf\r\n", currentAngle );
yong304 0:a1ef0a01d09f 226 // printf("gyroAngle = %6lf\r\n", gyroAngle );
yong304 0:a1ef0a01d09f 227 Thread::wait(2);
yong304 0:a1ef0a01d09f 228 end = timer.read_us();
yong304 0:a1ef0a01d09f 229 actuatedLED=1;
yong304 0:a1ef0a01d09f 230 // printf("Toggle the led takes %d us \r\n", end - begin);
yong304 0:a1ef0a01d09f 231
yong304 0:a1ef0a01d09f 232 }
yong304 0:a1ef0a01d09f 233 }