Cleaned up the code a bit
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- Lanyu
- Date:
- 2019-10-18
- Revision:
- 6:4bd7404af757
- Parent:
- 5:4530cd699129
- Child:
- 7:e871e2a28f08
File content as of revision 6:4bd7404af757:
#include "mbed.h" //#include "HIDScope.h" #include "QEI.h" #include "MODSERIAL.h" //#include "BiQuad.h" #include "FastPWM.h" #include <iostream> #include <math.h> //COMMUNICATING WITH PC #define SERIAL_BAUD 115200 Serial pc(USBTX, USBRX); Ticker EncTicker; // Ticker for encoder //MOTOR 1 //DONT HAVE TO BE CONNECTED BY WIRES, IS DONE BY THE HARDWARE ITSELF DigitalOut direction_1(D7); //tells motor 1 to turn CW (bool = false) or CCW (bool = true) PwmOut msignal_1(D6); //PWM signal to motor controller, higher PWM = higher avarage voltage DigitalIn but_1(D1); //D1 to BUT1 DigitalIn enc_1a(D10); //D10 to ENC1 A DigitalIn enc_1b(D9); //D9 to ENC1 B QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING); //MOTOR 2 //DigitalOut direction_2(D4); //tells motor 2 to turn CW or CCW //PwmOut msignal_2(D5); //PWM signal to motor controller, higher PWM = higher avarage voltage volatile float width; volatile float width_percent; const float period = 10.0; //sets period to 10ms volatile float on_time_1; volatile float off_time_1; volatile bool dir_1; //True = CCW, false = CW volatile float angle_1; //input wanted angle volatile float psi_1 = 0; //angle of motor_1 in radians volatile float psiabs_1 = 0; //absolute value of psi_1 #define M_PI acos(-1.0) const float to_rads =(2*M_PI); //2*Pi volatile int counts_1 = 0; // counts of Encoder_1 const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor) //const float GearRatio = 131.25; // GearRatio(Property of motor) const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count) // FUNCTIONS float GetWidthFromKeyboard() //Gets input from PC { start_width: float a; cin >> a; if(a>1) { pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n"); goto start_width; } else if(a<0) { pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n"); goto start_width; } else { pc.printf("\nYour input: [%f]\r\n\r\n--------\r\n\n",a); return a; } } float GetAngleFromKeyboard() //Gets input from PC { start_angle: float a; cin >> a; if(a>to_rads) { pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n"); goto start_angle; } else if(a< -to_rads) { pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n"); goto start_angle; } else if(a<0) { pc.printf("\nYour input: [%f] --> CW motion.\r\n\r\n--------\r\n\n",a); dir_1 = false; return a; } else { pc.printf("\nYour input: [%f] --> CCW motion.\r\n\r\n--------\r\n\n",a); dir_1 = true; return a; } } void PWM_MOTOR_1 (void) //Calculates on and off times { width = period * width_percent; on_time_1 = width; off_time_1 = period - width; } void MOTOR_1 () //runs the motor { msignal_1 = 1; // Turn motor on wait_ms(on_time_1); msignal_1 = 0; // Turn motor off wait_ms(off_time_1); } float Enc_1psi() //calculates the angle of motor 1 { psi_1 = ShaftResolution * counts_1; pc.printf("Angle of motor_1: %f radians\r\n",psi_1); return psi_1; } int main() { pc.baud(SERIAL_BAUD); pc.printf("--------\r\nWelcome!\r\n--------\r\n\n"); start_main: pc.printf("counts_1 = %i\r\n", counts_1); wait(0.5); pc.printf("Please imput your <Angle> value (-2pi,2pi), positive for CCW motion or negative for CW motion\r\n"); angle_1 = GetAngleFromKeyboard(); //(-2pi,2pi) wait(0.5); pc.printf("Please imput your <width> value in percentile of the period (0,1)\r\n"); width_percent = GetWidthFromKeyboard(); //in percentile (0,1) direction_1 = dir_1; PWM_MOTOR_1(); while (true) { counts_1 = Encoder_1.getPulses(); Enc_1psi(); MOTOR_1(); if (dir_1) // stop for CCW motion { if (psi_1 > angle_1) { pc.printf("--------\r\nReached Destination\r\n--------\r\n\n"); goto start_main; } } if (!dir_1) // stop for CW motion { if (psi_1 < angle_1) { pc.printf("--------\r\nReached Destination\r\n--------\r\n\n"); goto start_main; } } if (but_1 == 0) { pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n"); goto start_main; } } }