Hester van de Ven / Mbed 2 deprecated EncoderCode

Cleaned up the code a bit

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@1:b56499238c53, 2019-10-11 (annotated)

Committer:
Lanyu
Date:
Fri Oct 11 12:13:40 2019 +0000
Revision:
1:b56499238c53
Parent:
0:3c2e28fef203
Child:
2:a246ac8f0381
angle function is called by the encoder ticker now, amount of ticks 'n' varies with different called speeds of the motor, i.e. 0.1 -> ~800 and 0.5 -> 149. CHECK THIS!

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 1:b56499238c53 41 const float to_rads =((2*M_PI)/131); //contant for claculating one tick of the motor to radians, 8400 is a property of the motor -> 8400 counts per revolution
Lanyu 1:b56499238c53 42 volatile int n = 0;
Lanyu 0:3c2e28fef203 43
Lanyu 0:3c2e28fef203 44 // FUNCTIONS
Lanyu 0:3c2e28fef203 45 float GetValueFromKeyboard() //Gets input from PC
Lanyu 0:3c2e28fef203 46 {
Lanyu 0:3c2e28fef203 47 start:
Lanyu 0:3c2e28fef203 48
Lanyu 0:3c2e28fef203 49 float a;
Lanyu 0:3c2e28fef203 50 cin >> a;
Lanyu 0:3c2e28fef203 51 if(a>1)
Lanyu 0:3c2e28fef203 52 {
Lanyu 0:3c2e28fef203 53 pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
Lanyu 0:3c2e28fef203 54 goto start;
Lanyu 0:3c2e28fef203 55 }
Lanyu 0:3c2e28fef203 56 else if(a<-1)
Lanyu 0:3c2e28fef203 57 {
Lanyu 0:3c2e28fef203 58 pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
Lanyu 0:3c2e28fef203 59 goto start;
Lanyu 0:3c2e28fef203 60 }
Lanyu 0:3c2e28fef203 61 else if(a<0)
Lanyu 0:3c2e28fef203 62 {
Lanyu 0:3c2e28fef203 63 pc.printf("\nYour input: [%f] -> CW motion\r\n\r\n--------\r\n\n",a);
Lanyu 0:3c2e28fef203 64 dir_1 = false;
Lanyu 0:3c2e28fef203 65 a = abs(a); //otherwise width becomes negative, and it doesnt like that
Lanyu 0:3c2e28fef203 66 return a;
Lanyu 0:3c2e28fef203 67 }
Lanyu 0:3c2e28fef203 68 else
Lanyu 0:3c2e28fef203 69 {
Lanyu 0:3c2e28fef203 70 pc.printf("\nYour input: [%f] -> CCW motion\r\n\r\n--------\r\n\n",a);
Lanyu 0:3c2e28fef203 71 dir_1 = true;
Lanyu 0:3c2e28fef203 72 return a;
Lanyu 0:3c2e28fef203 73 }
Lanyu 0:3c2e28fef203 74 }
Lanyu 0:3c2e28fef203 75
Lanyu 0:3c2e28fef203 76
Lanyu 0:3c2e28fef203 77 void PWM_MOTOR_1 (void) //Calculates on and off times
Lanyu 0:3c2e28fef203 78 {
Lanyu 0:3c2e28fef203 79 width = period * width_percent;
Lanyu 0:3c2e28fef203 80 on_time_1 = width;
Lanyu 0:3c2e28fef203 81 off_time_1 = period - width;
Lanyu 0:3c2e28fef203 82 }
Lanyu 0:3c2e28fef203 83
Lanyu 0:3c2e28fef203 84 void MOTOR_1 () //runs the motor
Lanyu 0:3c2e28fef203 85 {
Lanyu 0:3c2e28fef203 86 msignal_1 = 1; // Turn motor on
Lanyu 0:3c2e28fef203 87 wait_ms(on_time_1);
Lanyu 0:3c2e28fef203 88 msignal_1 = 0; // Turn motor off
Lanyu 0:3c2e28fef203 89 wait_ms(off_time_1);
Lanyu 0:3c2e28fef203 90 }
Lanyu 0:3c2e28fef203 91
Lanyu 0:3c2e28fef203 92 float Enc_1Radians ()
Lanyu 0:3c2e28fef203 93 {
Lanyu 0:3c2e28fef203 94 if(a_1==a_1prev && b_1==b_1prev)
Lanyu 0:3c2e28fef203 95 {
Lanyu 0:3c2e28fef203 96 }
Lanyu 0:3c2e28fef203 97 else
Lanyu 0:3c2e28fef203 98 {
Lanyu 0:3c2e28fef203 99 psi_1prev = psi_1;
Lanyu 0:3c2e28fef203 100 if(dir_1)
Lanyu 0:3c2e28fef203 101 {
Lanyu 1:b56499238c53 102 n++;
Lanyu 0:3c2e28fef203 103 psi_1 = psi_1prev + to_rads;
Lanyu 0:3c2e28fef203 104 }
Lanyu 0:3c2e28fef203 105 else
Lanyu 0:3c2e28fef203 106 {
Lanyu 0:3c2e28fef203 107 psi_1 = psi_1prev - to_rads;
Lanyu 0:3c2e28fef203 108 }
Lanyu 0:3c2e28fef203 109 pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
Lanyu 0:3c2e28fef203 110 }
Lanyu 0:3c2e28fef203 111 return psi_1;
Lanyu 0:3c2e28fef203 112 }
Lanyu 1:b56499238c53 113
Lanyu 1:b56499238c53 114 void ReadEnc_1()
Lanyu 1:b56499238c53 115 {
Lanyu 1:b56499238c53 116 a_1prev = a_1;
Lanyu 1:b56499238c53 117 b_1prev = b_1;
Lanyu 1:b56499238c53 118 a_1 = enc_1a;
Lanyu 1:b56499238c53 119 b_1 = enc_1b;
Lanyu 1:b56499238c53 120
Lanyu 1:b56499238c53 121 Enc_1Radians ();
Lanyu 1:b56499238c53 122 //pc.printf("a_1i = %f, b_1i = %f\r\n", a_1, b_1);
Lanyu 1:b56499238c53 123 }
Lanyu 0:3c2e28fef203 124
Lanyu 0:3c2e28fef203 125 int main()
Lanyu 0:3c2e28fef203 126 {
Lanyu 0:3c2e28fef203 127 pc.baud(SERIAL_BAUD);
Lanyu 0:3c2e28fef203 128
Lanyu 0:3c2e28fef203 129 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
Lanyu 1:b56499238c53 130 EncTicker.attach(&ReadEnc_1, 0.01); //Ticker function to read Enc_1 to HIDScope
Lanyu 0:3c2e28fef203 131
Lanyu 0:3c2e28fef203 132 start_main:
Lanyu 1:b56499238c53 133 pc.printf("n = %i\r\n", n);
Lanyu 0:3c2e28fef203 134
Lanyu 0:3c2e28fef203 135 wait(0.5);
Lanyu 0:3c2e28fef203 136
Lanyu 0:3c2e28fef203 137 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 138 width_percent = GetValueFromKeyboard(); //in percentile (-1,1)
Lanyu 0:3c2e28fef203 139 direction_1 = dir_1;
Lanyu 0:3c2e28fef203 140 PWM_MOTOR_1();
Lanyu 0:3c2e28fef203 141
Lanyu 0:3c2e28fef203 142 while (true)
Lanyu 0:3c2e28fef203 143 {
Lanyu 0:3c2e28fef203 144 MOTOR_1();
Lanyu 0:3c2e28fef203 145 if (but_1 == 0)
Lanyu 0:3c2e28fef203 146 {
Lanyu 0:3c2e28fef203 147 pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 148 goto start_main;
Lanyu 0:3c2e28fef203 149 }
Lanyu 0:3c2e28fef203 150 }
Lanyu 0:3c2e28fef203 151 }