Hester van de Ven / Mbed 2 deprecated EncoderCode

Cleaned up the code a bit

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@6:4bd7404af757, 2019-10-18 (annotated)

Committer:
Lanyu
Date:
Fri Oct 18 07:55:25 2019 +0000
Revision:
6:4bd7404af757
Parent:
5:4530cd699129
Child:
7:e871e2a28f08
improved on the proof of concept for the angle input from last version, works great now, needs to be tested for bugs though

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 2:a246ac8f0381 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 2:a246ac8f0381 20 DigitalIn enc_1a(D10); //D10 to ENC1 A
Lanyu 2:a246ac8f0381 21 DigitalIn enc_1b(D9); //D9 to ENC1 B
Lanyu 3:46da963e60fb 22 QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING);
Lanyu 0:3c2e28fef203 23
Lanyu 0:3c2e28fef203 24 //MOTOR 2
Lanyu 0:3c2e28fef203 25 //DigitalOut direction_2(D4); //tells motor 2 to turn CW or CCW
Lanyu 0:3c2e28fef203 26 //PwmOut msignal_2(D5); //PWM signal to motor controller, higher PWM = higher avarage voltage
Lanyu 0:3c2e28fef203 27
Lanyu 0:3c2e28fef203 28 volatile float width;
Lanyu 0:3c2e28fef203 29 volatile float width_percent;
Lanyu 6:4bd7404af757 30 const float period = 10.0; //sets period to 10ms
Lanyu 0:3c2e28fef203 31 volatile float on_time_1;
Lanyu 0:3c2e28fef203 32 volatile float off_time_1;
Lanyu 0:3c2e28fef203 33 volatile bool dir_1; //True = CCW, false = CW
Lanyu 5:4530cd699129 34 volatile float angle_1; //input wanted angle
Lanyu 0:3c2e28fef203 35 volatile float psi_1 = 0; //angle of motor_1 in radians
Lanyu 5:4530cd699129 36 volatile float psiabs_1 = 0; //absolute value of psi_1
Lanyu 0:3c2e28fef203 37 #define M_PI acos(-1.0)
Lanyu 2:a246ac8f0381 38 const float to_rads =(2*M_PI); //2*Pi
Lanyu 2:a246ac8f0381 39 volatile int counts_1 = 0; // counts of Encoder_1
Lanyu 2:a246ac8f0381 40 const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor)
Lanyu 4:7640f5dd9375 41 //const float GearRatio = 131.25; // GearRatio(Property of motor)
Lanyu 4:7640f5dd9375 42 const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count)
Lanyu 0:3c2e28fef203 43
Lanyu 0:3c2e28fef203 44 // FUNCTIONS
Lanyu 5:4530cd699129 45 float GetWidthFromKeyboard() //Gets input from PC
Lanyu 0:3c2e28fef203 46 {
Lanyu 5:4530cd699129 47 start_width:
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 6:4bd7404af757 53 pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n");
Lanyu 5:4530cd699129 54 goto start_width;
Lanyu 0:3c2e28fef203 55 }
Lanyu 0:3c2e28fef203 56 else if(a<0)
Lanyu 0:3c2e28fef203 57 {
Lanyu 6:4bd7404af757 58 pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n");
Lanyu 6:4bd7404af757 59 goto start_width;
Lanyu 0:3c2e28fef203 60 }
Lanyu 0:3c2e28fef203 61 else
Lanyu 0:3c2e28fef203 62 {
Lanyu 6:4bd7404af757 63 pc.printf("\nYour input: [%f]\r\n\r\n--------\r\n\n",a);
Lanyu 0:3c2e28fef203 64 return a;
Lanyu 0:3c2e28fef203 65 }
Lanyu 0:3c2e28fef203 66 }
Lanyu 0:3c2e28fef203 67
Lanyu 5:4530cd699129 68 float GetAngleFromKeyboard() //Gets input from PC
Lanyu 5:4530cd699129 69 {
Lanyu 5:4530cd699129 70 start_angle:
Lanyu 5:4530cd699129 71
Lanyu 5:4530cd699129 72 float a;
Lanyu 5:4530cd699129 73 cin >> a;
Lanyu 5:4530cd699129 74 if(a>to_rads)
Lanyu 5:4530cd699129 75 {
Lanyu 6:4bd7404af757 76 pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n");
Lanyu 6:4bd7404af757 77 goto start_angle;
Lanyu 6:4bd7404af757 78 }
Lanyu 6:4bd7404af757 79 else if(a< -to_rads)
Lanyu 6:4bd7404af757 80 {
Lanyu 6:4bd7404af757 81 pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n");
Lanyu 5:4530cd699129 82 goto start_angle;
Lanyu 5:4530cd699129 83 }
Lanyu 5:4530cd699129 84 else if(a<0)
Lanyu 5:4530cd699129 85 {
Lanyu 6:4bd7404af757 86 pc.printf("\nYour input: [%f] --> CW motion.\r\n\r\n--------\r\n\n",a);
Lanyu 6:4bd7404af757 87 dir_1 = false;
Lanyu 6:4bd7404af757 88 return a;
Lanyu 5:4530cd699129 89 }
Lanyu 5:4530cd699129 90 else
Lanyu 5:4530cd699129 91 {
Lanyu 6:4bd7404af757 92 pc.printf("\nYour input: [%f] --> CCW motion.\r\n\r\n--------\r\n\n",a);
Lanyu 6:4bd7404af757 93 dir_1 = true;
Lanyu 5:4530cd699129 94 return a;
Lanyu 5:4530cd699129 95 }
Lanyu 5:4530cd699129 96 }
Lanyu 0:3c2e28fef203 97
Lanyu 0:3c2e28fef203 98 void PWM_MOTOR_1 (void) //Calculates on and off times
Lanyu 0:3c2e28fef203 99 {
Lanyu 0:3c2e28fef203 100 width = period * width_percent;
Lanyu 0:3c2e28fef203 101 on_time_1 = width;
Lanyu 0:3c2e28fef203 102 off_time_1 = period - width;
Lanyu 0:3c2e28fef203 103 }
Lanyu 0:3c2e28fef203 104
Lanyu 0:3c2e28fef203 105 void MOTOR_1 () //runs the motor
Lanyu 0:3c2e28fef203 106 {
Lanyu 0:3c2e28fef203 107 msignal_1 = 1; // Turn motor on
Lanyu 0:3c2e28fef203 108 wait_ms(on_time_1);
Lanyu 0:3c2e28fef203 109 msignal_1 = 0; // Turn motor off
Lanyu 0:3c2e28fef203 110 wait_ms(off_time_1);
Lanyu 0:3c2e28fef203 111 }
Lanyu 0:3c2e28fef203 112
Lanyu 2:a246ac8f0381 113 float Enc_1psi() //calculates the angle of motor 1
Lanyu 0:3c2e28fef203 114 {
Lanyu 2:a246ac8f0381 115 psi_1 = ShaftResolution * counts_1;
Lanyu 2:a246ac8f0381 116 pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
Lanyu 0:3c2e28fef203 117 return psi_1;
Lanyu 0:3c2e28fef203 118 }
Lanyu 0:3c2e28fef203 119
Lanyu 0:3c2e28fef203 120 int main()
Lanyu 0:3c2e28fef203 121 {
Lanyu 0:3c2e28fef203 122 pc.baud(SERIAL_BAUD);
Lanyu 0:3c2e28fef203 123
Lanyu 0:3c2e28fef203 124 pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 125
Lanyu 0:3c2e28fef203 126 start_main:
Lanyu 2:a246ac8f0381 127 pc.printf("counts_1 = %i\r\n", counts_1);
Lanyu 0:3c2e28fef203 128
Lanyu 0:3c2e28fef203 129 wait(0.5);
Lanyu 0:3c2e28fef203 130
Lanyu 6:4bd7404af757 131 pc.printf("Please imput your <Angle> value (-2pi,2pi), positive for CCW motion or negative for CW motion\r\n");
Lanyu 6:4bd7404af757 132 angle_1 = GetAngleFromKeyboard(); //(-2pi,2pi)
Lanyu 5:4530cd699129 133
Lanyu 5:4530cd699129 134 wait(0.5);
Lanyu 5:4530cd699129 135
Lanyu 6:4bd7404af757 136 pc.printf("Please imput your <width> value in percentile of the period (0,1)\r\n");
Lanyu 6:4bd7404af757 137 width_percent = GetWidthFromKeyboard(); //in percentile (0,1)
Lanyu 5:4530cd699129 138
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 2:a246ac8f0381 144 counts_1 = Encoder_1.getPulses();
Lanyu 2:a246ac8f0381 145 Enc_1psi();
Lanyu 0:3c2e28fef203 146 MOTOR_1();
Lanyu 6:4bd7404af757 147 if (dir_1) // stop for CCW motion
Lanyu 5:4530cd699129 148 {
Lanyu 6:4bd7404af757 149 if (psi_1 > angle_1)
Lanyu 6:4bd7404af757 150 {
Lanyu 6:4bd7404af757 151 pc.printf("--------\r\nReached Destination\r\n--------\r\n\n");
Lanyu 6:4bd7404af757 152 goto start_main;
Lanyu 6:4bd7404af757 153 }
Lanyu 6:4bd7404af757 154 }
Lanyu 6:4bd7404af757 155 if (!dir_1) // stop for CW motion
Lanyu 6:4bd7404af757 156 {
Lanyu 6:4bd7404af757 157 if (psi_1 < angle_1)
Lanyu 6:4bd7404af757 158 {
Lanyu 6:4bd7404af757 159 pc.printf("--------\r\nReached Destination\r\n--------\r\n\n");
Lanyu 6:4bd7404af757 160 goto start_main;
Lanyu 6:4bd7404af757 161 }
Lanyu 5:4530cd699129 162 }
Lanyu 0:3c2e28fef203 163 if (but_1 == 0)
Lanyu 0:3c2e28fef203 164 {
Lanyu 0:3c2e28fef203 165 pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
Lanyu 0:3c2e28fef203 166 goto start_main;
Lanyu 0:3c2e28fef203 167 }
Lanyu 0:3c2e28fef203 168 }
Lanyu 0:3c2e28fef203 169 }