Hester van de Ven / Mbed 2 deprecated EncoderCode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@0:3c2e28fef203, 2019-10-11 (annotated)

Committer:
Lanyu
Date:
Fri Oct 11 10:43:31 2019 +0000
Revision:
0:3c2e28fef203
Child:
1:b56499238c53
encoder outputs readings, readings arent translated well yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Lanyu 0:3c2e28fef203 1 #include "mbed.h"
Lanyu 0:3c2e28fef203 2 //#include "HIDScope.h"
Lanyu 0:3c2e28fef203 3 //#include "QEI.h"
Lanyu 0:3c2e28fef203 4 #include "MODSERIAL.h"
Lanyu 0:3c2e28fef203 5 //#include "BiQuad.h"
Lanyu 0:3c2e28fef203 6 #include "FastPWM.h"
Lanyu 0:3c2e28fef203 7 #include <iostream>
Lanyu 0:3c2e28fef203 8 #include <math.h>
Lanyu 0:3c2e28fef203 9
Lanyu 0:3c2e28fef203 10 //COMMUNICATING WITH PC
Lanyu 0:3c2e28fef203 11 #define SERIAL_BAUD 115200
Lanyu 0:3c2e28fef203 12 Serial pc(USBTX, USBRX);
Lanyu 0:3c2e28fef203 13
Lanyu 0:3c2e28fef203 14 Ticker EncTicker; // Ticker for encoder
Lanyu 0:3c2e28fef203 15
Lanyu 0:3c2e28fef203 16 //MOTOR 1 //DONT HAVE TO BE CONNECTED BY WIRES, IS DONE BY THE HARDWARE ITSELF
Lanyu 0:3c2e28fef203 17 DigitalOut direction_1(D7); //tells motor 1 to turn CW (bool = false) or CCW (bool = true)
Lanyu 0:3c2e28fef203 18 PwmOut msignal_1(D6); //PWM signal to motor controller, higher PWM = higher avarage voltage
Lanyu 0:3c2e28fef203 19 DigitalIn but_1(D1); //D1 to BUT1
Lanyu 0:3c2e28fef203 20 DigitalIn enc_1a(D10); //D13 to ENC1 A
Lanyu 0:3c2e28fef203 21 DigitalIn enc_1b(D9); //D12 to ENC1 B
Lanyu 0:3c2e28fef203 22
Lanyu 0:3c2e28fef203 23 //MOTOR 2
Lanyu 0:3c2e28fef203 24 //DigitalOut direction_2(D4); //tells motor 2 to turn CW or CCW
Lanyu 0:3c2e28fef203 25 //PwmOut msignal_2(D5); //PWM signal to motor controller, higher PWM = higher avarage voltage
Lanyu 0:3c2e28fef203 26
Lanyu 0:3c2e28fef203 27 volatile float width;
Lanyu 0:3c2e28fef203 28 volatile float width_percent;
Lanyu 0:3c2e28fef203 29 float period = 10.0; //sets period to 10ms
Lanyu 0:3c2e28fef203 30 volatile float on_time_1;
Lanyu 0:3c2e28fef203 31 volatile float off_time_1;
Lanyu 0:3c2e28fef203 32 volatile bool dir_1; //True = CCW, false = CW
Lanyu 0:3c2e28fef203 33 volatile float a_1 = enc_1a; // enc_a1 signal
Lanyu 0:3c2e28fef203 34 volatile float b_1 = enc_1b; // enc b_1 signal
Lanyu 0:3c2e28fef203 35 volatile float a_1prev = enc_1a; // enc_a1 previous signal
Lanyu 0:3c2e28fef203 36 volatile float b_1prev = enc_1b; // enc b_1 previous signal
Lanyu 0:3c2e28fef203 37 volatile int ticks_1 = 0; //counts times the encoder ticked from the start position
Lanyu 0:3c2e28fef203 38 volatile float psi_1 = 0; //angle of motor_1 in radians
Lanyu 0:3c2e28fef203 39 volatile float psi_1prev = 0; //precious angle of motor_1 in radians
Lanyu 0:3c2e28fef203 40 #define M_PI acos(-1.0)
Lanyu 0:3c2e28fef203 41 const float to_rads =((2*M_PI)/8400); //contant for claculating one tick of the motor to radians, 8400 is a property of the motor -> 8400 counts per revolution
Lanyu 0:3c2e28fef203 42
Lanyu 0:3c2e28fef203 43 // FUNCTIONS
Lanyu 0:3c2e28fef203 44 float GetValueFromKeyboard() //Gets input from PC
Lanyu 0:3c2e28fef203 45 {
Lanyu 0:3c2e28fef203 46 start:
Lanyu 0:3c2e28fef203 47
Lanyu 0:3c2e28fef203 48 float a;
Lanyu 0:3c2e28fef203 49 cin >> a;
Lanyu 0:3c2e28fef203 50 if(a>1)
Lanyu 0:3c2e28fef203 51 {
Lanyu 0:3c2e28fef203 52 pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
Lanyu 0:3c2e28fef203 53 goto start;
Lanyu 0:3c2e28fef203 54 }
Lanyu 0:3c2e28fef203 55 else if(a<-1)
Lanyu 0:3c2e28fef203 56 {
Lanyu 0:3c2e28fef203 57 pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
Lanyu 0:3c2e28fef203 58 goto start;
Lanyu 0:3c2e28fef203 59 }
Lanyu 0:3c2e28fef203 60 else if(a<0)
Lanyu 0:3c2e28fef203 61 {
Lanyu 0:3c2e28fef203 62 pc.printf("\nYour input: [%f] -> CW motion\r\n\r\n--------\r\n\n",a);
Lanyu 0:3c2e28fef203 63 dir_1 = false;
Lanyu 0:3c2e28fef203 64 a = abs(a); //otherwise width becomes negative, and it doesnt like that
Lanyu 0:3c2e28fef203 65 return a;
Lanyu 0:3c2e28fef203 66 }
Lanyu 0:3c2e28fef203 67 else
Lanyu 0:3c2e28fef203 68 {
Lanyu 0:3c2e28fef203 69 pc.printf("\nYour input: [%f] -> CCW motion\r\n\r\n--------\r\n\n",a);
Lanyu 0:3c2e28fef203 70 dir_1 = true;
Lanyu 0:3c2e28fef203 71 return a;
Lanyu 0:3c2e28fef203 72 }
Lanyu 0:3c2e28fef203 73 }
Lanyu 0:3c2e28fef203 74
Lanyu 0:3c2e28fef203 75 //void GetDirectionFromkeyboard ()
Lanyu 0:3c2e28fef203 76 //{
Lanyu 0:3c2e28fef203 77 // start_dir:
Lanyu 0:3c2e28fef203 78 //
Lanyu 0:3c2e28fef203 79 // int b;
Lanyu 0:3c2e28fef203 80 // cin >> b;
Lanyu 0:3c2e28fef203 81 // switch (b)
Lanyu 0:3c2e28fef203 82 // {
Lanyu 0:3c2e28fef203 83 // case 1:
Lanyu 0:3c2e28fef203 84 // {
Lanyu 0:3c2e28fef203 85 // pc.printf("\nYour input: [1] -> CCW motion\r\n\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 86 // dir_1 = true;
Lanyu 0:3c2e28fef203 87 // break;
Lanyu 0:3c2e28fef203 88 // }
Lanyu 0:3c2e28fef203 89 // case 0:
Lanyu 0:3c2e28fef203 90 // {
Lanyu 0:3c2e28fef203 91 // pc.printf("\nYour input: [0] -> CW motion\r\n\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 92 // dir_1 = false;
Lanyu 0:3c2e28fef203 93 // break;
Lanyu 0:3c2e28fef203 94 // }
Lanyu 0:3c2e28fef203 95 // default:
Lanyu 0:3c2e28fef203 96 // {
Lanyu 0:3c2e28fef203 97 // pc.printf("\nYour input was invalid, please input 1 for CCW or 0 for CW motion.\r\n");
Lanyu 0:3c2e28fef203 98 // goto start_dir;
Lanyu 0:3c2e28fef203 99 // }
Lanyu 0:3c2e28fef203 100 // }
Lanyu 0:3c2e28fef203 101 //}
Lanyu 0:3c2e28fef203 102
Lanyu 0:3c2e28fef203 103 void PWM_MOTOR_1 (void) //Calculates on and off times
Lanyu 0:3c2e28fef203 104 {
Lanyu 0:3c2e28fef203 105 width = period * width_percent;
Lanyu 0:3c2e28fef203 106 on_time_1 = width;
Lanyu 0:3c2e28fef203 107 off_time_1 = period - width;
Lanyu 0:3c2e28fef203 108 }
Lanyu 0:3c2e28fef203 109
Lanyu 0:3c2e28fef203 110 void MOTOR_1 () //runs the motor
Lanyu 0:3c2e28fef203 111 {
Lanyu 0:3c2e28fef203 112 msignal_1 = 1; // Turn motor on
Lanyu 0:3c2e28fef203 113 wait_ms(on_time_1);
Lanyu 0:3c2e28fef203 114 msignal_1 = 0; // Turn motor off
Lanyu 0:3c2e28fef203 115 wait_ms(off_time_1);
Lanyu 0:3c2e28fef203 116 }
Lanyu 0:3c2e28fef203 117
Lanyu 0:3c2e28fef203 118 void ReadEnc_1()
Lanyu 0:3c2e28fef203 119 {
Lanyu 0:3c2e28fef203 120 a_1prev = a_1;
Lanyu 0:3c2e28fef203 121 b_1prev = b_1;
Lanyu 0:3c2e28fef203 122 a_1 = enc_1a;
Lanyu 0:3c2e28fef203 123 b_1 = enc_1b;
Lanyu 0:3c2e28fef203 124
Lanyu 0:3c2e28fef203 125 //pc.printf("a_1i = %f, b_1i = %f\r\n", a_1, b_1);
Lanyu 0:3c2e28fef203 126 }
Lanyu 0:3c2e28fef203 127
Lanyu 0:3c2e28fef203 128 float Enc_1Radians ()
Lanyu 0:3c2e28fef203 129 {
Lanyu 0:3c2e28fef203 130 if(a_1==a_1prev && b_1==b_1prev)
Lanyu 0:3c2e28fef203 131 {
Lanyu 0:3c2e28fef203 132 }
Lanyu 0:3c2e28fef203 133 else
Lanyu 0:3c2e28fef203 134 {
Lanyu 0:3c2e28fef203 135 psi_1prev = psi_1;
Lanyu 0:3c2e28fef203 136 if(dir_1)
Lanyu 0:3c2e28fef203 137 {
Lanyu 0:3c2e28fef203 138 psi_1 = psi_1prev + to_rads;
Lanyu 0:3c2e28fef203 139 }
Lanyu 0:3c2e28fef203 140 else
Lanyu 0:3c2e28fef203 141 {
Lanyu 0:3c2e28fef203 142 psi_1 = psi_1prev - to_rads;
Lanyu 0:3c2e28fef203 143 }
Lanyu 0:3c2e28fef203 144 pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
Lanyu 0:3c2e28fef203 145 }
Lanyu 0:3c2e28fef203 146 return psi_1;
Lanyu 0:3c2e28fef203 147 }
Lanyu 0:3c2e28fef203 148
Lanyu 0:3c2e28fef203 149 int main()
Lanyu 0:3c2e28fef203 150 {
Lanyu 0:3c2e28fef203 151 pc.baud(SERIAL_BAUD);
Lanyu 0:3c2e28fef203 152
Lanyu 0:3c2e28fef203 153 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 154 EncTicker.attach(&ReadEnc_1, 0.0001); //Ticker function to read Enc_1 to HIDScope
Lanyu 0:3c2e28fef203 155
Lanyu 0:3c2e28fef203 156 start_main:
Lanyu 0:3c2e28fef203 157
Lanyu 0:3c2e28fef203 158 // pc.printf("Please imput your desired direction value, either 1 for CCW or 0 for CW movement\r\n");
Lanyu 0:3c2e28fef203 159 // GetDirectionFromkeyboard();
Lanyu 0:3c2e28fef203 160
Lanyu 0:3c2e28fef203 161
Lanyu 0:3c2e28fef203 162 wait(0.5);
Lanyu 0:3c2e28fef203 163
Lanyu 0:3c2e28fef203 164 pc.printf("Please imput your <width> value in percentile of the period (-1,1), positive for CCW motion or negative for CW motion\r\n");
Lanyu 0:3c2e28fef203 165 width_percent = GetValueFromKeyboard(); //in percentile (-1,1)
Lanyu 0:3c2e28fef203 166 direction_1 = dir_1;
Lanyu 0:3c2e28fef203 167 PWM_MOTOR_1();
Lanyu 0:3c2e28fef203 168
Lanyu 0:3c2e28fef203 169 while (true)
Lanyu 0:3c2e28fef203 170 {
Lanyu 0:3c2e28fef203 171 Enc_1Radians ();
Lanyu 0:3c2e28fef203 172 MOTOR_1();
Lanyu 0:3c2e28fef203 173 if (but_1 == 0)
Lanyu 0:3c2e28fef203 174 {
Lanyu 0:3c2e28fef203 175 pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 176 goto start_main;
Lanyu 0:3c2e28fef203 177 }
Lanyu 0:3c2e28fef203 178 }
Lanyu 0:3c2e28fef203 179 }