Steven Mitchell
/
HelloWorld_IKS01A2
afdfdf
Diff: main.cpp
- Revision:
- 19:29d3c7caea66
- Parent:
- 18:9b9ec0b35b45
- Child:
- 20:babcf777607b
--- a/main.cpp Tue Nov 19 00:42:38 2019 +0000 +++ b/main.cpp Tue Nov 19 03:14:08 2019 +0000 @@ -42,6 +42,12 @@ #include <iostream> #include <cmath> + + +struct Vec3 { + float x, y, z; +}; + /* Instantiate the expansion board */ static XNucleoIKS01A2 *mems_expansion_board = XNucleoIKS01A2::instance(D14, D15, D4, D5); @@ -53,27 +59,21 @@ - -struct Vec3 { - float x, y, z; -}; +DigitalOut M1_step(D3); +DigitalOut M1_dir(D2); +DigitalOut M2_step(D5); +DigitalOut M2_dir(D4); -static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f}; +DigitalIn button1(D6); +DigitalIn button2(D7); -Vec3 get_gyro() +void delay(float time) //delay function debounce buttons { - Vec3 vec3; - - int32_t axes[3]; - acc_gyro->get_g_axes(axes); + volatile int i; + for(i=0; i<1000000*time; i++); +} - vec3.x = axes[0]/1000.0f - gyro_calibrate.x; - vec3.y = axes[1]/1000.0f - gyro_calibrate.y; - vec3.z = axes[2]/1000.0f - gyro_calibrate.z; - - return vec3; -} Vec3 get_accel() { @@ -88,18 +88,18 @@ return vec3; } - - void print_vec3(const char* str, const Vec3& vec3) { std::cout << str << vec3.x << " " << vec3.y << " " << vec3.z << "\r\n"; } -/* Simple main function */ + + + + int main() { - /* Enable all sensors */ magnetometer->enable(); accelerometer->enable(); @@ -109,49 +109,114 @@ //Timer t; //float dt = 0.02f; - std::cout << "\r\n--- Starting new run ---\r\n"; + //std::cout << "\r\n--- Starting new run ---\r\n"; //Vec3 orientation = {}; //float sens = 0.488f; - + Vec3 vec3, tilt; - + // 180/pi -> converts radians to degrees. float rad_to_deg = 57.2957795131f; + M1_dir = 1; + M2_dir = 0; + for(;;) { - //t.start(); + vec3 = get_accel(); + + // We only need x or y; I left the others here for documentation. + //tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg; + tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg; + //tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg; + +//control stepper motor 1 + + float degree = std::abs(tilt.y); + + //print_vec3("acceleration: ", tilt); + //std::cout << "\r\n" << std::flush; - //Vec3 gyro_reading = get_gyro(); + int numOfSteps = degree / 0.45; - //orientation.x += (gyro_reading.x * dt * sens); - //orientation.y += (gyro_reading.y * dt * sens); - //orientation.z += (gyro_reading.z * dt * sens); + if(tilt.y > 0) { + M1_dir = 0; + M2_dir = 1; + } else { + M1_dir = 1; + M2_dir = 0; + } - //print_vec3("orientation: ", orientation); - - - - vec3 = get_accel(); - - tilt.x = atan(vec3.x / sqrtf(vec3.y*vec3.y + vec3.z*vec3.z)) * rad_to_deg; - tilt.y = atan(vec3.y / sqrtf(vec3.x*vec3.x + vec3.z*vec3.z)) * rad_to_deg; - tilt.z = atan(sqrtf(vec3.y*vec3.y + vec3.x*vec3.x) / vec3.z) * rad_to_deg; + for(int i = 0; i < numOfSteps; i++) { + M2_step = 1; + M1_step = 1; + delay(0.005); + M1_step = 0; + M2_step = 0; + delay(0.005); + } + } + +} + - print_vec3("acceleration: ", tilt); + + + + + + + + + + + + - std::cout << "\r\n" << std::flush; + + + + + + + + + + + + - t.stop(); - if (dt - t.read() > 0) wait(dt - t.read()); - dt = t.read(); - } + + + + + + + + + + + + + + + + + + + + + + + + + @@ -161,30 +226,120 @@ - //for(;;) { - //-0.42 -1.61 1.05 - //magnetometer->get_m_axes(axes); - //std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + + + + + + + - //accelerometer->get_x_axes(axes); - //std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + + + + + + + + - //acc_gyro->get_x_axes(axes); - //std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; +//static const Vec3 gyro_calibrate = {-0.42f, -1.61f, 1.05f}; + +//Vec3 get_gyro() +//{ +// Vec3 vec3; + +// int32_t axes[3]; +// acc_gyro->get_g_axes(axes); + +// vec3.x = axes[0]/1000.0f - gyro_calibrate.x; +// vec3.y = axes[1]/1000.0f - gyro_calibrate.y; +// vec3.z = axes[2]/1000.0f - gyro_calibrate.z; + +// return vec3; +//} - //acc_gyro->get_g_axes(axes); - //std::cout << "LSM6DSL [gyro/dps]: " << axes[0]/1000.0f + 0.42f << " " << axes[1]/1000.0f + 1.61f << " " << axes[2]/1000.0f-1.05f << "\r\n"; + - //print_vec3("gyro/dps: ", get_gyro()); - //std::cout << "\r\n" << std::flush; +/* Simple main function */ +//int main() +//{ + + + + +//for(;;) { +//t.start(); + + +//Vec3 gyro_reading = get_gyro(); + +//orientation.x += (gyro_reading.x * dt * sens); +//orientation.y += (gyro_reading.y * dt * sens); +//orientation.z += (gyro_reading.z * dt * sens); + +//print_vec3("orientation: ", orientation); + + + + + + + + + + + +//tilt.x + + - //wait(1.5); - //} -} +//std::cout << "\r\n" << std::flush; + +//t.stop(); +//if (dt - t.read() > 0) wait(dt - t.read()); +//dt = t.read(); +//} + + + + + + + + + +//for(;;) { +//-0.42 -1.61 1.05 +//magnetometer->get_m_axes(axes); +//std::cout << "LSM303AGR [mag/gauss]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + +//accelerometer->get_x_axes(axes); +//std::cout << "LSM303AGR [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + + +//acc_gyro->get_x_axes(axes); +//std::cout << "LSM6DSL [acc/g]: " << axes[0]/1000.0f << " " << axes[1]/1000.0f << " " << axes[2]/1000.0f << "\r\n"; + + +//acc_gyro->get_g_axes(axes); +//std::cout << "LSM6DSL [gyro/dps]: " << axes[0]/1000.0f + 0.42f << " " << axes[1]/1000.0f + 1.61f << " " << axes[2]/1000.0f-1.05f << "\r\n"; + + + + + +//print_vec3("gyro/dps: ", get_gyro()); +// + + +//wait(1.5); +//} +//}