Nicolas Borla
PES2 Firmware for mbed2
Dependencies: Stepper mbed Servo LSM9DS1_Library SDFileSystem
Revision 0:41afd907b7bd, committed 2021-03-12
- Comitter:
- boro
- Date:
- Fri Mar 12 13:06:52 2021 +0000
- Commit message:
- d
Changed in this revision
diff -r 000000000000 -r 41afd907b7bd EncoderCounter.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.cpp Fri Mar 12 13:06:52 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();
+}
+
+
+
diff -r 000000000000 -r 41afd907b7bd EncoderCounter.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/EncoderCounter.h Fri Mar 12 13:06:52 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_ */
+
+
+
diff -r 000000000000 -r 41afd907b7bd LSM9DS1_Library.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS1_Library.lib Fri Mar 12 13:06:52 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/boro/code/LSM9DS1_Library/#0e123c149624
diff -r 000000000000 -r 41afd907b7bd SDFileSystem.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SDFileSystem.lib Fri Mar 12 13:06:52 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/boro/code/SDFileSystem/#bb9899ceb521
diff -r 000000000000 -r 41afd907b7bd Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Fri Mar 12 13:06:52 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/jdenkers/code/Servo/#352133517ccc
diff -r 000000000000 -r 41afd907b7bd Stepper.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Stepper.lib Fri Mar 12 13:06:52 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/boro/code/Stepper/#510f88bf20c6
diff -r 000000000000 -r 41afd907b7bd main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Mar 12 13:06:52 2021 +0000
@@ -0,0 +1,296 @@
+#include "mbed.h"
+#include "LSM9DS1.h"
+#include "Servo.h"
+#include "EncoderCounter.h"
+#include "Stepper.h"
+#include "SDFileSystem.h"
+
+DigitalOut myled(LED1);
+
+// create object IMU
+LSM9DS1 imu(PC_9, PA_8, 0xD6, 0x3C);
+
+// create servo objects
+Servo S0(PB_2);
+Servo S1(PC_8);
+Servo S2(PC_6);
+
+DigitalOut enable(PB_15);
+
+// 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);
+
+// initialise PWM
+Stepper stepperMotor(PB_14,PC_4); // step, dir
+Stepper stepperMotor2(PB_12,PA_15); // config 2
+
+DigitalIn user_button(USER_BUTTON);
+
+SDFileSystem sd(PC_12, PC_11, PC_10, PD_2, "sd"); // mosi miso clk cs
+AnalogIn ain(PC_2);
+DigitalOut enableStepper(PB_1);
+
+// define write function
+void sdWrite(float a){
+ printf("start write SD\r\n");
+
+ FILE *fp = fopen("/sd/mydir/sdtest.txt", "a");
+ if(fp == NULL) {
+ error("Could not open file for write\r\n");
+ }
+ fprintf(fp, "analog measure %f\r\n",a);
+ fclose(fp);
+
+ printf("write succesfull\r\n\n");
+}
+
+int main() {
+ // test comunication
+ imu.begin();
+ if (!imu.begin()) {
+ printf("Failed to communicate with LSM9DS1.\r\n");
+ }
+
+ // initialise and test Servo
+ S0.Enable(1000,20000);
+ S1.Enable(1000,20000);
+ S2.Enable(1000,20000);
+
+ // initialise PWM
+ pwm_motor1.period(0.0005f);// 0.5ms 2KHz
+ pwm_motor1.write(0.5f);
+ pwm_motor2.period(0.0005f);// 0.5ms 2KHz
+ pwm_motor2.write(0.5f);
+ pwm_motor3.period(0.0005f);// 0.5ms 2KHz
+ pwm_motor3.write(0.5f);
+
+ // initialise stepper
+ stepperMotor.setSpeed(10000);
+ stepperMotor.rotate(stepperMotor.CW);
+ stepperMotor.stop();
+ stepperMotor.setPositionZero();
+
+ stepperMotor2.setSpeed(10000);
+ stepperMotor2.rotate(stepperMotor.CW);
+ stepperMotor2.stop();
+ stepperMotor2.setPositionZero();
+
+ int i = 0;
+
+ bool buttonNow = false;
+ bool buttonBefore = false;
+ bool done = false;
+
+ enableStepper = 1;
+
+
+ while(1) {
+
+
+ buttonNow = !user_button;
+
+ if(buttonNow && !buttonBefore){i+=1; done = false;}
+ if(i>7){i=0;}
+
+ switch (i){
+ case 1:
+ // ===================
+ // IMU
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nIMU test\r\n======================\r\n\r\n");
+ for(int i=0; i<5; i++){
+ // do a measure
+ imu.readAccel();
+ imu.readMag();
+ imu.readGyro();
+
+ // print values to console
+ printf("gyro: %.5f %.5f %.5f [rad/s]\r\n", imu.gyroX,imu.gyroY,imu.gyroZ);
+ printf("accel: %.5f %.5f %.5f [m/s^2]\r\n", imu.accX,imu.accY,imu.accZ);
+ printf("mag: %.5f %.5f %.5f [Gauss]\r\n\n", imu.magX,imu.magY,imu.magZ);
+ wait(0.5f);
+ }
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+ case 2:
+ // ===================
+ // Servo
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nServo test\r\n======================\r\n\r\n");
+ for(int j=0; j<2000; j+=50){
+ printf("position %d\r\n",j);
+ S0.SetPosition(j);
+ S1.SetPosition(j);
+ S2.SetPosition(j);
+
+ wait(0.1f);
+ }
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+ case 3:
+ // ===================
+ // DC - Motor1
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nDC - Motor1 test\r\n======================\r\n\r\n");
+ enable = 1;
+ for(int j=0; j<20; j+=1){
+ pwm_motor1.write(0.7f);
+ pwm_motor2.write(0.7f);
+ pwm_motor3.write(0.7f);
+ // display encoder values to console
+ printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+ wait(0.1f);
+ }
+ enable = 0;
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+ case 4:
+ // ===================
+ // DC - Motor2
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nDC - Motor2 test\r\n======================\r\n\r\n");
+ enable = 1;
+ for(int j=0; j<20; j+=1){
+ pwm_motor1.write(0.7f);
+ pwm_motor2.write(0.7f);
+ pwm_motor3.write(0.7f);
+ // display encoder values to console
+ printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+ wait(0.1f);
+ }
+ enable = 0;
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+ case 5:
+ // ===================
+ // DC - Motor3
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nDC - Motor3 test\r\n======================\r\n\r\n");
+ enable = 1;
+ for(int j=0; j<20; j+=1){
+ pwm_motor1.write(0.7f);
+ pwm_motor2.write(0.7f);
+ pwm_motor3.write(0.7f);
+ // display encoder values to console
+ printf("counter1 = %d counter2 = %d counter3 = %d\r\n",counter1.read(), counter2.read(), counter3.read());
+ wait(0.1f);
+ }
+ enable = 0;
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+ case 6:
+ // ===================
+ // Stepper
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nStepper test\r\n======================\r\n\r\n");
+ // test stepper motor
+ enableStepper = 0;
+ wait(0.5f);
+ printf("stepper enabled\r\n");
+ stepperMotor.goesTo(20*2048);
+ stepperMotor2.goesTo(20*2048);
+ wait(3.0f);
+ stepperMotor.stop();
+ stepperMotor2.stop();
+ stepperMotor.setPositionZero();
+ stepperMotor2.setPositionZero();
+ enableStepper = 1;
+ printf("stepper done\r\n");
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+ case 7:
+ // ===================
+ // SD
+ // ===================
+ if(!done){
+ printf("\r\n\r\n======================\r\nSD test\r\n======================\r\n\r\n");
+
+ // crearte folder
+ mkdir("/sd/mydir", 0777);
+
+ // create file
+ printf("start write SD\r\n");
+ FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\r\n");
+ }
+ fprintf(fp, "analog measure\r\n");
+ fprintf(fp, "==============\r\n");
+ fclose(fp);
+
+ // write 2 times an analog value
+ for(int i=0; i<2; i++){
+ sdWrite(ain.read());
+ wait(2.0f);
+ }
+ // read from SD card
+ printf("Reading from SD card...");
+ fp = fopen("/sd/mydir/sdtest.txt", "r");
+ if (fp != NULL) {
+ char c = fgetc(fp);
+ if (c == 'a')
+ printf("success!\n");
+ else
+ printf("incorrect char (%c)!\n", c);
+ fclose(fp);
+ } else {
+ printf("failed!\n");
+ }
+
+ done = true;
+ break;
+ } else {
+ break;
+ }
+
+
+ default:
+ printf("\r\n\r\n======================\r\nPause\r\n======================\r\n\r\n");
+ break;
+ }
+
+
+ myled = 1;
+
+ wait(0.1); // 0.5s
+
+ myled = 0;
+ }
+}
diff -r 000000000000 -r 41afd907b7bd mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Mar 12 13:06:52 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file