Hester van de Ven / Mbed 2 deprecated EncoderCode

Cleaned up the code a bit

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@8:dd89293949e3, 2019-10-18 (annotated)

Committer:
HestervdVen
Date:
Fri Oct 18 08:23:23 2019 +0000
Revision:
8:dd89293949e3
Parent:
7:e871e2a28f08 
blah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
HestervdVen 8:dd89293949e3 1 //test test
HestervdVen 8:dd89293949e3 2
Lanyu 0:3c2e28fef203 3 #include "mbed.h"
Lanyu 2:a246ac8f0381 4 #include "QEI.h"
Lanyu 0:3c2e28fef203 5 #include "MODSERIAL.h"
Lanyu 0:3c2e28fef203 6 #include "FastPWM.h"
Lanyu 0:3c2e28fef203 7 #include <iostream>
Lanyu 0:3c2e28fef203 8 #include <math.h>
HestervdVen 7:e871e2a28f08 9 //#include "HIDScope.h"
HestervdVen 7:e871e2a28f08 10 //#include "BiQuad.h"
Lanyu 0:3c2e28fef203 11
Lanyu 0:3c2e28fef203 12 //COMMUNICATING WITH PC
Lanyu 0:3c2e28fef203 13 #define SERIAL_BAUD 115200
HestervdVen 7:e871e2a28f08 14 #define M_PI acos(-1.0)
HestervdVen 7:e871e2a28f08 15
Lanyu 0:3c2e28fef203 16 Serial pc(USBTX, USBRX);
Lanyu 0:3c2e28fef203 17
Lanyu 0:3c2e28fef203 18 Ticker EncTicker; // Ticker for encoder
Lanyu 0:3c2e28fef203 19
Lanyu 0:3c2e28fef203 20 //MOTOR 1 //DONT HAVE TO BE CONNECTED BY WIRES, IS DONE BY THE HARDWARE ITSELF
Lanyu 0:3c2e28fef203 21 DigitalOut direction_1(D7); //tells motor 1 to turn CW (bool = false) or CCW (bool = true)
Lanyu 0:3c2e28fef203 22 PwmOut msignal_1(D6); //PWM signal to motor controller, higher PWM = higher avarage voltage
Lanyu 0:3c2e28fef203 23 DigitalIn but_1(D1); //D1 to BUT1
Lanyu 2:a246ac8f0381 24 DigitalIn enc_1a(D10); //D10 to ENC1 A
Lanyu 2:a246ac8f0381 25 DigitalIn enc_1b(D9); //D9 to ENC1 B
Lanyu 3:46da963e60fb 26 QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING);
Lanyu 0:3c2e28fef203 27
Lanyu 0:3c2e28fef203 28 //MOTOR 2
Lanyu 0:3c2e28fef203 29 //DigitalOut direction_2(D4); //tells motor 2 to turn CW or CCW
Lanyu 0:3c2e28fef203 30 //PwmOut msignal_2(D5); //PWM signal to motor controller, higher PWM = higher avarage voltage
Lanyu 0:3c2e28fef203 31
HestervdVen 7:e871e2a28f08 32 const float period = 10.0; //sets period to 10ms
HestervdVen 7:e871e2a28f08 33 const float to_rads =(2*M_PI); //2*Pi
HestervdVen 7:e871e2a28f08 34 const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor)
HestervdVen 7:e871e2a28f08 35 //const float GearRatio = 131.25; // GearRatio(Property of motor)
HestervdVen 7:e871e2a28f08 36 const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count)
Lanyu 0:3c2e28fef203 37 volatile float width;
Lanyu 0:3c2e28fef203 38 volatile float width_percent;
Lanyu 0:3c2e28fef203 39 volatile float on_time_1;
Lanyu 0:3c2e28fef203 40 volatile float off_time_1;
Lanyu 5:4530cd699129 41 volatile float angle_1; //input wanted angle
Lanyu 0:3c2e28fef203 42 volatile float psi_1 = 0; //angle of motor_1 in radians
Lanyu 5:4530cd699129 43 volatile float psiabs_1 = 0; //absolute value of psi_1
Lanyu 2:a246ac8f0381 44 volatile int counts_1 = 0; // counts of Encoder_1
HestervdVen 7:e871e2a28f08 45 volatile bool dir_1; //True = CCW, false = CW
Lanyu 0:3c2e28fef203 46
Lanyu 0:3c2e28fef203 47 // FUNCTIONS
Lanyu 5:4530cd699129 48 float GetWidthFromKeyboard() //Gets input from PC
Lanyu 0:3c2e28fef203 49 {
Lanyu 5:4530cd699129 50 start_width:
Lanyu 0:3c2e28fef203 51
Lanyu 0:3c2e28fef203 52 float a;
Lanyu 0:3c2e28fef203 53 cin >> a;
Lanyu 0:3c2e28fef203 54 if(a>1)
Lanyu 0:3c2e28fef203 55 {
Lanyu 6:4bd7404af757 56 pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n");
Lanyu 5:4530cd699129 57 goto start_width;
Lanyu 0:3c2e28fef203 58 }
Lanyu 0:3c2e28fef203 59 else if(a<0)
Lanyu 0:3c2e28fef203 60 {
Lanyu 6:4bd7404af757 61 pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n");
Lanyu 6:4bd7404af757 62 goto start_width;
Lanyu 0:3c2e28fef203 63 }
Lanyu 0:3c2e28fef203 64 else
Lanyu 0:3c2e28fef203 65 {
Lanyu 6:4bd7404af757 66 pc.printf("\nYour input: [%f]\r\n\r\n--------\r\n\n",a);
Lanyu 0:3c2e28fef203 67 return a;
Lanyu 0:3c2e28fef203 68 }
Lanyu 0:3c2e28fef203 69 }
Lanyu 0:3c2e28fef203 70
Lanyu 5:4530cd699129 71 float GetAngleFromKeyboard() //Gets input from PC
Lanyu 5:4530cd699129 72 {
Lanyu 5:4530cd699129 73 start_angle:
Lanyu 5:4530cd699129 74
Lanyu 5:4530cd699129 75 float a;
Lanyu 5:4530cd699129 76 cin >> a;
Lanyu 5:4530cd699129 77 if(a>to_rads)
Lanyu 5:4530cd699129 78 {
Lanyu 6:4bd7404af757 79 pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n");
Lanyu 6:4bd7404af757 80 goto start_angle;
Lanyu 6:4bd7404af757 81 }
Lanyu 6:4bd7404af757 82 else if(a< -to_rads)
Lanyu 6:4bd7404af757 83 {
Lanyu 6:4bd7404af757 84 pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n");
Lanyu 5:4530cd699129 85 goto start_angle;
Lanyu 5:4530cd699129 86 }
Lanyu 5:4530cd699129 87 else if(a<0)
Lanyu 5:4530cd699129 88 {
Lanyu 6:4bd7404af757 89 pc.printf("\nYour input: [%f] --> CW motion.\r\n\r\n--------\r\n\n",a);
Lanyu 6:4bd7404af757 90 dir_1 = false;
Lanyu 6:4bd7404af757 91 return a;
Lanyu 5:4530cd699129 92 }
Lanyu 5:4530cd699129 93 else
Lanyu 5:4530cd699129 94 {
Lanyu 6:4bd7404af757 95 pc.printf("\nYour input: [%f] --> CCW motion.\r\n\r\n--------\r\n\n",a);
Lanyu 6:4bd7404af757 96 dir_1 = true;
Lanyu 5:4530cd699129 97 return a;
Lanyu 5:4530cd699129 98 }
Lanyu 5:4530cd699129 99 }
Lanyu 0:3c2e28fef203 100
Lanyu 0:3c2e28fef203 101 void PWM_MOTOR_1 (void) //Calculates on and off times
Lanyu 0:3c2e28fef203 102 {
Lanyu 0:3c2e28fef203 103 width = period * width_percent;
Lanyu 0:3c2e28fef203 104 on_time_1 = width;
Lanyu 0:3c2e28fef203 105 off_time_1 = period - width;
Lanyu 0:3c2e28fef203 106 }
Lanyu 0:3c2e28fef203 107
Lanyu 0:3c2e28fef203 108 void MOTOR_1 () //runs the motor
Lanyu 0:3c2e28fef203 109 {
Lanyu 0:3c2e28fef203 110 msignal_1 = 1; // Turn motor on
Lanyu 0:3c2e28fef203 111 wait_ms(on_time_1);
Lanyu 0:3c2e28fef203 112 msignal_1 = 0; // Turn motor off
Lanyu 0:3c2e28fef203 113 wait_ms(off_time_1);
Lanyu 0:3c2e28fef203 114 }
Lanyu 0:3c2e28fef203 115
Lanyu 2:a246ac8f0381 116 float Enc_1psi() //calculates the angle of motor 1
Lanyu 0:3c2e28fef203 117 {
Lanyu 2:a246ac8f0381 118 psi_1 = ShaftResolution * counts_1;
Lanyu 2:a246ac8f0381 119 pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
Lanyu 0:3c2e28fef203 120 return psi_1;
Lanyu 0:3c2e28fef203 121 }
Lanyu 0:3c2e28fef203 122
Lanyu 0:3c2e28fef203 123 int main()
Lanyu 0:3c2e28fef203 124 {
Lanyu 0:3c2e28fef203 125 pc.baud(SERIAL_BAUD);
Lanyu 0:3c2e28fef203 126
Lanyu 0:3c2e28fef203 127 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 128
Lanyu 0:3c2e28fef203 129 start_main:
HestervdVen 7:e871e2a28f08 130 pc.printf("counts_1 = %i\r\n", counts_1); //For own insight, not necessary in final code
Lanyu 0:3c2e28fef203 131
Lanyu 0:3c2e28fef203 132 wait(0.5);
Lanyu 0:3c2e28fef203 133
Lanyu 6:4bd7404af757 134 pc.printf("Please imput your <Angle> value (-2pi,2pi), positive for CCW motion or negative for CW motion\r\n");
Lanyu 6:4bd7404af757 135 angle_1 = GetAngleFromKeyboard(); //(-2pi,2pi)
Lanyu 5:4530cd699129 136
Lanyu 5:4530cd699129 137 wait(0.5);
Lanyu 5:4530cd699129 138
Lanyu 6:4bd7404af757 139 pc.printf("Please imput your <width> value in percentile of the period (0,1)\r\n");
Lanyu 6:4bd7404af757 140 width_percent = GetWidthFromKeyboard(); //in percentile (0,1)
Lanyu 5:4530cd699129 141
HestervdVen 7:e871e2a28f08 142 direction_1 = dir_1; //give direction to port
Lanyu 0:3c2e28fef203 143 PWM_MOTOR_1();
Lanyu 0:3c2e28fef203 144
Lanyu 0:3c2e28fef203 145 while (true)
Lanyu 0:3c2e28fef203 146 {
Lanyu 2:a246ac8f0381 147 counts_1 = Encoder_1.getPulses();
Lanyu 2:a246ac8f0381 148 Enc_1psi();
Lanyu 0:3c2e28fef203 149 MOTOR_1();
Lanyu 6:4bd7404af757 150 if (dir_1) // stop for CCW motion
Lanyu 5:4530cd699129 151 {
Lanyu 6:4bd7404af757 152 if (psi_1 > angle_1)
Lanyu 6:4bd7404af757 153 {
Lanyu 6:4bd7404af757 154 pc.printf("--------\r\nReached Destination\r\n--------\r\n\n");
Lanyu 6:4bd7404af757 155 goto start_main;
Lanyu 6:4bd7404af757 156 }
Lanyu 6:4bd7404af757 157 }
Lanyu 6:4bd7404af757 158 if (!dir_1) // stop for CW motion
Lanyu 6:4bd7404af757 159 {
Lanyu 6:4bd7404af757 160 if (psi_1 < angle_1)
Lanyu 6:4bd7404af757 161 {
Lanyu 6:4bd7404af757 162 pc.printf("--------\r\nReached Destination\r\n--------\r\n\n");
Lanyu 6:4bd7404af757 163 goto start_main;
Lanyu 6:4bd7404af757 164 }
Lanyu 5:4530cd699129 165 }
Lanyu 0:3c2e28fef203 166 if (but_1 == 0)
Lanyu 0:3c2e28fef203 167 {
Lanyu 0:3c2e28fef203 168 pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 169 goto start_main;
Lanyu 0:3c2e28fef203 170 }
Lanyu 0:3c2e28fef203 171 }
Lanyu 0:3c2e28fef203 172 }