Nicolas Borla
/
PES2_mbed_os
PES2_mbed_os_6
Revision 0:5d4d21d56334, committed 2021-03-12
- Comitter:
- boro
- Date:
- Fri Mar 12 13:04:33 2021 +0000
- Child:
- 1:0e396592f33d
- Commit message:
- controller added;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.gitignore Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,4 @@ +.build +.mbed +projectfiles +*.py*
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.cpp Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,129 @@ +#include "Controller.h" + +using namespace std; + +const float Controller::PERIOD = 0.001f; // period of 1 ms +const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution +const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s] +const float Controller::KN = 40.0f; // speed constant in [rpm/V] +const float Controller::KP = 0.2f; // speed control parameter +const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V] +const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle +const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle + +Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, + EncoderCounter& counterLeft, EncoderCounter& counterRight) : + pwmLeft(pwmLeft), pwmRight(pwmRight), + counterLeft(counterLeft), counterRight(counterRight) { + + // Initialisieren der PWM Ausgaenge + + pwmLeft.period(0.00005f); // PWM Periode von 50 us + pwmLeft = 0.5f; // Duty-Cycle von 50% + pwmRight.period(0.00005f); // PWM Periode von 50 us + pwmRight = 0.5f; // Duty-Cycle von 50% + + // Initialisieren von lokalen Variabeln + + previousValueCounterLeft = counterLeft.read(); + previousValueCounterRight = counterRight.read(); + + speedLeftFilter.setPeriod(PERIOD); + speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); + + speedRightFilter.setPeriod(PERIOD); + speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY); + + desiredSpeedLeft = 0.0f; + desiredSpeedRight = 0.0f; + + actualSpeedLeft = 0.0f; + actualSpeedRight = 0.0f; + + actualAngleLeft = 0.0f; + actualAngleRight = 0.0f; + + // Starten des periodischen Tasks + thread.start(callback(this, &Controller::run)); + ticker.attach(callback(this, &Controller::sendThreadFlag), PERIOD); +} + +Controller::~Controller() +{ + ticker.detach(); // Stoppt den periodischen Task +} + + +void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) +{ + this->desiredSpeedLeft = desiredSpeedLeft; +} + +void Controller::setDesiredSpeedRight(float desiredSpeedRight) +{ + this->desiredSpeedRight = desiredSpeedRight; +} + +float Controller::getSpeedLeft() +{ + return actualSpeedLeft; +} + +float Controller::getSpeedRight() +{ + return actualSpeedRight; +} + +/** + * This method is called by the ticker timer interrupt service routine. + * It sends a flag to the thread to make it run again. + */ +void Controller::sendThreadFlag() { + + thread.flags_set(threadFlag); +} + +void Controller::run() { + + while(true) { + + // wait for the periodic signal + + ThisThread::flags_wait_any(threadFlag); + + // calculate actual speed of motors in [rpm] + + short valueCounterLeft = counterLeft.read(); + short valueCounterRight = counterRight.read(); + + short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft; + short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight; + + previousValueCounterLeft = valueCounterLeft; + previousValueCounterRight = valueCounterRight; + + actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f); + actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f); + + actualAngleLeft = actualAngleLeft + actualSpeedLeft/60.0f*PERIOD; + actualAngleRight = actualAngleRight + actualSpeedRight/60.0f*PERIOD; + + // calculate motor phase voltages + + float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN; + float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN; + + // calculate, limit and set duty cycles + + float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE; + if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE; + else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE; + pwmLeft.write(dutyCycleLeft); + + float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE; + if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE; + else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE; + pwmRight.write(dutyCycleRight); + + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.h Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,58 @@ +#ifndef CONTROLLER_H_ +#define CONTROLLER_H_ +#include <cstdlib> +#include <mbed.h> +#include "EncoderCounter.h" +#include "LowpassFilter.h" +#include "ThreadFlag.h" + +class Controller +{ + +public: + + Controller(PwmOut& pwmLeft, PwmOut& pwmRight, + EncoderCounter& counterLeft, EncoderCounter& counterRight); + + virtual ~Controller(); + void setDesiredSpeedLeft(float desiredSpeedLeft); + void setDesiredSpeedRight(float desiredSpeedRight); + float getSpeedLeft(); + float getSpeedRight(); + +private: + + static const float PERIOD; + static const float COUNTS_PER_TURN; + static const float LOWPASS_FILTER_FREQUENCY; + static const float KN; + static const float KP; + static const float MAX_VOLTAGE; + static const float MIN_DUTY_CYCLE; + static const float MAX_DUTY_CYCLE; + + PwmOut& pwmLeft; + PwmOut& pwmRight; + EncoderCounter& counterLeft; + EncoderCounter& counterRight; + short previousValueCounterLeft; + short previousValueCounterRight; + LowpassFilter speedLeftFilter; + LowpassFilter speedRightFilter; + float desiredSpeedLeft; + float desiredSpeedRight; + float actualSpeedLeft; + float actualSpeedRight; + float actualAngleLeft; + float actualAngleRight; + + ThreadFlag threadFlag; + Thread thread; + Ticker ticker; + + void sendThreadFlag(); + void run(); + +}; + +#endif /* CONTROLLER_H_ */ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderCounter.cpp Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,171 @@ +/* + * EncoderCounter.cpp + * Copyright (c) 2018, ZHAW + * All rights reserved. + */ + +#include "EncoderCounter.h" + +using namespace std; + +/** + * Creates and initializes the driver to read the quadrature + * encoder counter of the STM32 microcontroller. + * @param a the input pin for the channel A. + * @param b the input pin for the channel B. + */ +EncoderCounter::EncoderCounter(PinName a, PinName b) { + + // check pins + + if ((a == PA_0) && (b == PA_1)) { + + // pinmap OK for TIM2 CH1 and CH2 + + TIM = TIM2; + + // configure general purpose I/O registers + + GPIOA->MODER &= ~GPIO_MODER_MODER0; // reset port A0 + GPIOA->MODER |= GPIO_MODER_MODER0_1; // set alternate mode of port A0 + GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR0; // reset pull-up/pull-down on port A0 + GPIOA->PUPDR |= GPIO_PUPDR_PUPDR0_1; // set input as pull-down + GPIOA->AFR[0] &= ~(0xF << 4*0); // reset alternate function of port A0 + GPIOA->AFR[0] |= 1 << 4*0; // set alternate funtion 1 of port A0 + + GPIOA->MODER &= ~GPIO_MODER_MODER1; // reset port A1 + GPIOA->MODER |= GPIO_MODER_MODER1_1; // set alternate mode of port A1 + GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR1; // reset pull-up/pull-down on port A1 + GPIOA->PUPDR |= GPIO_PUPDR_PUPDR1_1; // set input as pull-down + GPIOA->AFR[0] &= ~(0xF << 4*1); // reset alternate function of port A1 + GPIOA->AFR[0] |= 1 << 4*1; // set alternate funtion 1 of port A1 + + // configure reset and clock control registers + + RCC->APB1RSTR |= RCC_APB1RSTR_TIM2RST; //reset TIM2 controller + RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM2RST; + + RCC->APB1ENR |= RCC_APB1ENR_TIM2EN; // TIM2 clock enable + + } else if ((a == PA_6) && (b == PC_7)) { + + // pinmap OK for TIM3 CH1 and CH2 + + TIM = TIM3; + + // configure reset and clock control registers + + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // manually enable port C (port A enabled by mbed library) + + // configure general purpose I/O registers + + GPIOA->MODER &= ~GPIO_MODER_MODER6; // reset port A6 + GPIOA->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port A6 + GPIOA->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port A6 + GPIOA->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down + GPIOA->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port A6 + GPIOA->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port A6 + + GPIOC->MODER &= ~GPIO_MODER_MODER7; // reset port C7 + GPIOC->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port C7 + GPIOC->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port C7 + GPIOC->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down + GPIOC->AFR[0] &= ~0xF0000000; // reset alternate function of port C7 + GPIOC->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port C7 + + // configure reset and clock control registers + + RCC->APB1RSTR |= RCC_APB1RSTR_TIM3RST; //reset TIM3 controller + RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM3RST; + + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // TIM3 clock enable + + } else if ((a == PB_6) && (b == PB_7)) { + + // pinmap OK for TIM4 CH1 and CH2 + + TIM = TIM4; + + // configure reset and clock control registers + + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // manually enable port B (port A enabled by mbed library) + + // configure general purpose I/O registers + + GPIOB->MODER &= ~GPIO_MODER_MODER6; // reset port B6 + GPIOB->MODER |= GPIO_MODER_MODER6_1; // set alternate mode of port B6 + GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR6; // reset pull-up/pull-down on port B6 + GPIOB->PUPDR |= GPIO_PUPDR_PUPDR6_1; // set input as pull-down + GPIOB->AFR[0] &= ~(0xF << 4*6); // reset alternate function of port B6 + GPIOB->AFR[0] |= 2 << 4*6; // set alternate funtion 2 of port B6 + + GPIOB->MODER &= ~GPIO_MODER_MODER7; // reset port B7 + GPIOB->MODER |= GPIO_MODER_MODER7_1; // set alternate mode of port B7 + GPIOB->PUPDR &= ~GPIO_PUPDR_PUPDR7; // reset pull-up/pull-down on port B7 + GPIOB->PUPDR |= GPIO_PUPDR_PUPDR7_1; // set input as pull-down + GPIOB->AFR[0] &= ~0xF0000000; // reset alternate function of port B7 + GPIOB->AFR[0] |= 2 << 4*7; // set alternate funtion 2 of port B7 + + // configure reset and clock control registers + + RCC->APB1RSTR |= RCC_APB1RSTR_TIM4RST; //reset TIM4 controller + RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM4RST; + + RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // TIM4 clock enable + + } else { + + printf("pinmap not found for peripheral\n"); + } + + // configure general purpose timer 3 or 4 + + TIM->CR1 = 0x0000; // counter disable + TIM->CR2 = 0x0000; // reset master mode selection + TIM->SMCR = TIM_SMCR_SMS_1 | TIM_SMCR_SMS_0; // counting on both TI1 & TI2 edges + TIM->CCMR1 = TIM_CCMR1_CC2S_0 | TIM_CCMR1_CC1S_0; + TIM->CCMR2 = 0x0000; // reset capture mode register 2 + TIM->CCER = TIM_CCER_CC2E | TIM_CCER_CC1E; + TIM->CNT = 0x0000; // reset counter value + TIM->ARR = 0xFFFF; // auto reload register + TIM->CR1 = TIM_CR1_CEN; // counter enable +} + +EncoderCounter::~EncoderCounter() {} + +/** + * Resets the counter value to zero. + */ +void EncoderCounter::reset() { + + TIM->CNT = 0x0000; +} + +/** + * Resets the counter value to a given offset value. + * @param offset the offset value to reset the counter to. + */ +void EncoderCounter::reset(int16_t offset) { + + TIM->CNT = -offset; +} + +/** + * Reads the quadrature encoder counter value. + * @return the quadrature encoder counter as a signed 16-bit integer value. + */ +int16_t EncoderCounter::read() { + + return static_cast<int16_t>(-TIM->CNT); +} + +/** + * The empty operator is a shorthand notation of the <code>read()</code> method. + */ +EncoderCounter::operator int16_t() { + + return read(); +} + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EncoderCounter.h Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,37 @@ +/* + * EncoderCounter.h + * Copyright (c) 2018, ZHAW + * All rights reserved. + */ + +#ifndef ENCODER_COUNTER_H_ +#define ENCODER_COUNTER_H_ + +#include <cstdlib> +#include <stdint.h> +#include <mbed.h> + +/** + * This class implements a driver to read the quadrature + * encoder counter of the STM32 microcontroller. + */ +class EncoderCounter { + + public: + + EncoderCounter(PinName a, PinName b); + virtual ~EncoderCounter(); + void reset(); + void reset(int16_t offset); + int16_t read(); + operator int16_t(); + + private: + + TIM_TypeDef* TIM; +}; + +#endif /* ENCODER_COUNTER_H_ */ + + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LowpassFilter.cpp Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,113 @@ +/* + * LowpassFilter.cpp + * Copyright (c) 2018, ZHAW + * All rights reserved. + */ + +#include <cmath> +#include "LowpassFilter.h" + +using namespace std; + +/** + * Creates a LowpassFilter object with a default cutoff frequency of 1000 [rad/s]. + */ +LowpassFilter::LowpassFilter() { + + period = 1.0f; + frequency = 1000.0f; + + a11 = (1.0f+frequency*period)*exp(-frequency*period); + a12 = period*exp(-frequency*period); + a21 = -frequency*frequency*period*exp(-frequency*period); + a22 = (1.0f-frequency*period)*exp(-frequency*period); + b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; + b2 = period*exp(-frequency*period); + + x1 = 0.0f; + x2 = 0.0f; +} + +/** + * Deletes the LowpassFilter object. + */ +LowpassFilter::~LowpassFilter() {} + +/** + * Resets the filtered value to zero. + */ +void LowpassFilter::reset() { + + x1 = 0.0f; + x2 = 0.0f; +} + +/** + * Resets the filtered value to a given value. + * @param value the value to reset the filter to. + */ +void LowpassFilter::reset(float value) { + + x1 = value/frequency/frequency; + x2 = (x1-a11*x1-b1*value)/a12; +} + +/** + * Sets the sampling period of the filter. + * This is typically the sampling period of the periodic task of a controller that uses this filter. + * @param the sampling period, given in [s]. + */ +void LowpassFilter::setPeriod(float period) { + + this->period = period; + + a11 = (1.0f+frequency*period)*exp(-frequency*period); + a12 = period*exp(-frequency*period); + a21 = -frequency*frequency*period*exp(-frequency*period); + a22 = (1.0f-frequency*period)*exp(-frequency*period); + b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; + b2 = period*exp(-frequency*period); +} + +/** + * Sets the cutoff frequency of this filter. + * @param frequency the cutoff frequency of the filter in [rad/s]. + */ +void LowpassFilter::setFrequency(float frequency) { + + this->frequency = frequency; + + a11 = (1.0f+frequency*period)*exp(-frequency*period); + a12 = period*exp(-frequency*period); + a21 = -frequency*frequency*period*exp(-frequency*period); + a22 = (1.0f-frequency*period)*exp(-frequency*period); + b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; + b2 = period*exp(-frequency*period); +} + +/** + * Gets the current cutoff frequency of this filter. + * @return the current cutoff frequency in [rad/s]. + */ +float LowpassFilter::getFrequency() { + + return frequency; +} + +/** + * Filters a value. + * @param value the original unfiltered value. + * @return the filtered value. + */ +float LowpassFilter::filter(float value) { + + float x1old = x1; + float x2old = x2; + + x1 = a11*x1old+a12*x2old+b1*value; + x2 = a21*x1old+a22*x2old+b2*value; + + return frequency*frequency*x1; +} + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LowpassFilter.h Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,40 @@ +/* + * LowpassFilter.h + * Copyright (c) 2018, ZHAW + * All rights reserved. + */ + +#ifndef LOWPASS_FILTER_H_ +#define LOWPASS_FILTER_H_ + +#include <cstdlib> + +/** + * This class implements a time-discrete 2nd order lowpass filter for a series of data values. + * This filter can typically be used within a periodic task that takes measurements that need + * to be filtered, like speed or position values. + */ +class LowpassFilter { + + public: + + LowpassFilter(); + virtual ~LowpassFilter(); + void reset(); + void reset(float value); + void setPeriod(float period); + void setFrequency(float frequency); + float getFrequency(); + float filter(float value); + + private: + + float period; + float frequency; + float a11, a12, a21, a22, b1, b2; + float x1, x2; +}; + +#endif /* LOWPASS_FILTER_H_ */ + +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/jdenkers/code/Servo/#352133517ccc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ThreadFlag.cpp Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,54 @@ +/* + * ThreadFlag.cpp + * Copyright (c) 2020, ZHAW + * All rights reserved. + */ + +#include "ThreadFlag.h" + +using namespace std; + +unsigned int ThreadFlag::threadFlags = 0; + +/** + * Creates a signal object and assignes a unique flag. + */ +ThreadFlag::ThreadFlag() { + + mutex.lock(); + + unsigned int n = 0; + while ((((1 << n) & threadFlags) > 0) && (n < 30)) n++; + threadFlag = (1 << n); + + mutex.unlock(); +} + +/** + * Deletes the signal object and releases the assigned flag. + */ +ThreadFlag::~ThreadFlag() { + + mutex.lock(); + + threadFlags &= ~threadFlag; + + mutex.unlock(); +} + +/** + * Gets the assigned thread flag. + */ +unsigned int ThreadFlag::read() { + + return threadFlag; +} + +/** + * The empty operator is a shorthand notation of the <code>read()</code> method. + */ +ThreadFlag::operator unsigned int() { + + return read(); +} +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ThreadFlag.h Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,33 @@ +/* + * ThreadFlag.h + * Copyright (c) 2020, ZHAW + * All rights reserved. + */ + +#ifndef THREAD_FLAG_H_ +#define THREAD_FLAG_H_ + +#include <cstdlib> +#include <mbed.h> + +/** + * This class manages the handling of unique thread flags to trigger rtos threads. + */ +class ThreadFlag { + + public: + + ThreadFlag(); + virtual ~ThreadFlag(); + virtual unsigned int read(); + operator unsigned int(); + + private: + + static unsigned int threadFlags; // variable that holds all assigned thread flags + unsigned int threadFlag; // thread flag of this object + Mutex mutex; // mutex to lock critical sections +}; + +#endif /* THREAD_FLAG_H_ */ +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,119 @@ +/* mbed Microcontroller Library + * Copyright (c) 2019 ARM Limited + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "mbed.h" +#include "platform/mbed_thread.h" +#include "SDBlockDevice.h" +#include "FATFileSystem.h" +#include "EncoderCounter.h" +#include "Servo.h" +#include "Controller.h" + + +// Blinking rate in milliseconds +#define BLINKING_RATE_MS 500 + + +int main() +{ + + DigitalIn user_button(USER_BUTTON); + + // initialise PWM + PwmOut pwm_motor1(PB_13); + PwmOut pwm_motor2(PA_9); + PwmOut pwm_motor3(PA_10); + + // crete Encoder read objects + EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B) + EncoderCounter counter2(PB_6, PB_7); + EncoderCounter counter3(PA_0, PA_1); + + // create controller + Controller controller(pwm_motor1, pwm_motor2, counter1, counter1); + + DigitalOut enable(PB_15); + + // create servo objects + Servo S0(PB_2); + Servo S1(PC_8); + Servo S2(PC_6); + + SDBlockDevice sd(PC_12, PC_11, PC_10, PD_2); + printf("BlockDevice created\r\n"); + FATFileSystem fs("fs", &sd); + + // Initialise the digital pin LED1 as an output + DigitalOut myled(LED1); + + + + // initialise PWM + pwm_motor1.period(0.00005f);// 0.05ms 20KHz + pwm_motor1.write(0.5f); + pwm_motor2.period(0.00005f);// 0.05ms 20KHz + pwm_motor2.write(0.5f); + pwm_motor3.period(0.00005f);// 0.05ms 20KHz + pwm_motor3.write(0.5f); + + // initialise and test Servo + S0.Enable(1000,20000); + S1.Enable(1000,20000); + S2.Enable(1000,20000); + + printf("Test writing... "); + FILE* fp = fopen("/fs/data.csv", "w"); + fprintf(fp, "test %.5f\r\n",1.23); + fclose(fp); + printf("done\r\n"); + + printf("Test reading... "); + // read from SD card + fp = fopen("/fs/data.csv", "r"); + if (fp != NULL) { + char c = fgetc(fp); + if (c == 't') + printf("done\r\n"); + else + printf("incorrect char (%c)!\n", c); + fclose(fp); + } else { + printf("Reading failed!\n"); + } + + // enable driver DC motors + enable = 1; + + while (true) { + + if(!user_button) { + // LED off, set controller speed, pwm2, position servo + myled = 0; + controller.setDesiredSpeedLeft(50.0f); + controller.setDesiredSpeedRight(50.0f); + pwm_motor3.write(0.9f); + + S0.SetPosition(1200); + S1.SetPosition(1200); + S2.SetPosition(1200); + + } else { + // LED on, reset controller speed, pwm2, position servo + myled = 1; + controller.setDesiredSpeedLeft(0.0f); + controller.setDesiredSpeedRight(0.0f); + pwm_motor3.write(0.5f); + + S0.SetPosition(1900); + S1.SetPosition(1900); + S2.SetPosition(1900); + + } + + printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read()); + + thread_sleep_for(BLINKING_RATE_MS); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#cf4f12a123c05fcae83fc56d76442015cb8a39e9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed_app.json Fri Mar 12 13:04:33 2021 +0000 @@ -0,0 +1,7 @@ +{ + "target_overrides": { + "NUCLEO_F446RE": { + "target.components_add": ["SD"] + } + } +}