motor aansturing
Dependencies: Encoder HIDScope MODSERIAL mbed-dsp mbed
main.cpp
- Committer:
- DominiqueC
- Date:
- 2014-10-14
- Revision:
- 1:82a5126adc56
- Parent:
- 0:1d02168661ec
File content as of revision 1:82a5126adc56:
/***************************************/ /* */ /* BRONCODE GROEP 5, MODULE 9, 2014 */ /* *****-THE SLAP-****** */ /* */ /* -Dominique Clevers */ /* -Rianne van Dommelen */ /* -Daan de Muinck Keizer */ /* -David den Houting */ /* -Marjolein Thijssen */ /***************************************/ #include "mbed.h" #include "encoder.h" #include "MODSERIAL.h" #include "HIDScope.h" //MAXIMALE UITWIJKING BEPALEN /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ void keep_in_range(float * in, float min, float max); /** variable to show when a new loop can be started*/ /** volatile means that it can be changed in an */ /** interrupt routine, and that that change is vis-*/ /** ible in the main loop. */ volatile bool looptimerflag; /** function called by Ticker "looptimer" */ /** variable 'looptimerflag' is set to 'true' */ /** each time the looptimer expires. */ void setlooptimerflag(void) { looptimerflag = true; } int main() { //LOCAL VARIABLES /*Potmeter input*/ AnalogIn potmeter(PTC2); /* Encoder, using my encoder library */ /* First pin should be PTDx or PTAx */ /* because those pins can be used as */ /* InterruptIn */ Encoder motor1(PTD0,PTC9); /* MODSERIAL to get non-blocking Serial*/ MODSERIAL pc(USBTX,USBRX); /* PWM control to motor */ PwmOut pwm_motor(PTA12); /* Direction pin */ DigitalOut motordir(PTD3); /* variable to store setpoint in */ float setpoint; /* variable to store pwm value in*/ float pwm_to_motor; //START OF CODE /*Set the baudrate (use this number in RealTerm too! */ pc.baud(230400); /*Create a ticker, and let it call the */ /*function 'setlooptimerflag' every 0.01s */ Ticker looptimer; looptimer.attach(setlooptimerflag,0.01); //INFINITE LOOP while(1) { /* Wait until looptimer flag is true. */ /* '!=' means not-is-equal */ while(looptimerflag != true); /* Clear the looptimerflag, otherwise */ /* the program would simply continue */ /* without waitingin the next iteration*/ looptimerflag = false; /* Read potmeter value, apply some math */ /* to get useful setpoint value */ setpoint = (potmeter.read()-0.5)*2000; /* Print setpoint and current position to serial terminal*/ pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition()); /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */ pwm_to_motor = (setpoint - motor1.getPosition())*.001; /* Coerce pwm value if outside range */ /* Not strictly needed here, but useful */ /* if doing other calculations with pwm value */ keep_in_range(&pwm_to_motor, -1,1); /* Control the motor direction pin. based on */ /* the sign of your pwm value. If your */ /* motor keeps spinning when running this code */ /* you probably need to swap the motor wires, */ /* or swap the 'write(1)' and 'write(0)' below */ if(pwm_to_motor > 0) motordir.write(1); else motordir.write(0); //WRITE VALUE TO MOTOR /* Take the absolute value of the PWM to send */ /* to the motor. */ pwm_motor.write(abs(pwm_to_motor)); } } //coerces value 'in' to min or max when exceeding those values //if you'd like to understand the statement below take a google for //'ternary operators'. void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; } //AANSTUREN MOTOR (POSITIE EN SNELHEID) #define TSAMP 0.01 #define K_P (0.1) #define K_I (0.03 *TSAMP) #define K_D (0.001 /TSAMP) #define I_LIMIT 1. #define M1_PWM PTC8 #define M1_DIR PTC9 #define M2_PWM PTA5 #define M2_DIR PTA4 //#define POT_AVG 50 void clamp(float * in, float min, float max); float pid(float setpoint, float measurement); volatile bool looptimerflag; //float potsamples[POT_AVG]; HIDScope scope(6); void setlooptimerflag(void) { looptimerflag = true; } int main() { AnalogIn potmeter(PTC2); //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter Encoder motor1(PTD0,PTA13); /*PwmOut to motor driver*/ PwmOut pwm_motor(M2_PWM); //10kHz PWM frequency pwm_motor.period_us(75); DigitalOut motordir(M2_DIR); Ticker looptimer; looptimer.attach(setlooptimerflag,TSAMP); while(1) { int16_t setpoint; float new_pwm; /*wait until timer has elapsed*/ while(!looptimerflag); looptimerflag = false; //clear flag /*potmeter value: 0-1*/ setpoint = (potmeter.read()-.5)*500; /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/ new_pwm = pid(setpoint, motor1.getPosition()); clamp(&new_pwm, -1,1); scope.set(0, setpoint); scope.set(4, new_pwm); scope.set(5, motor1.getPosition()); // ch 1, 2 and 3 set in pid controller */ scope.send(); if(new_pwm < 0) motordir = 0; else motordir = 1; pwm_motor.write(abs(new_pwm)); } } //clamps value 'in' to min or max when exceeding those values //if you'd like to understand the statement below take a google for //'ternary operators'. void clamp(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; } float pid(float setpoint, float measurement) { float error; static float prev_error = 0; float out_p = 0; static float out_i = 0; float out_d = 0; error = setpoint-measurement; out_p = error*K_P; out_i += error*K_I; out_d = (error-prev_error)*K_D; clamp(&out_i,-I_LIMIT,I_LIMIT); prev_error = error; scope.set(1,out_p); scope.set(2,out_i); scope.set(3,out_d); return out_p + out_i + out_d; } //POSITIE EN SNELHEID UITLEZEN OP PC int main() { while(1) //Loop { /** Make encoder object. * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn * Second pin can be any digital input */ Encoder motor1(PTD0,PTC9); /*Use USB serial to send values*/ MODSERIAL pc(USBTX,USBRX); /*Set baud rate to 115200*/ pc.baud(115200); while(1) { //Loop /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/ wait(0.2); /** print position (integer) and speed (float) to the PC*/ pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed()); } }