Numero Uno / Mbed 2 deprecated PID_VelocityExample

motor controller with P velocity control

Dependencies:   HIDScope PID QEI mbed EMG

Fork of PID_VelocityExample by Aaron Berk

main.cpp@9:07189a75e979, 2015-10-06 (annotated)

Committer:
ewoud
Date:
Tue Oct 06 10:17:13 2015 +0000
Revision:
9:07189a75e979
Parent:
8:55ca92c0e39d
Child:
12:d7bb475bb82d
added second motor and deadzone support

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aberk 0:9bca35ae9c6b 1 //****************************************************************************/
aberk 0:9bca35ae9c6b 2 // Includes
aberk 0:9bca35ae9c6b 3 #include "PID.h"
aberk 0:9bca35ae9c6b 4 #include "QEI.h"
ewoud 9:07189a75e979 5 #include "HIDScope.h"
ewoud 9:07189a75e979 6 #include "inits.h" // all globals, pin and variable initialization
aberk 0:9bca35ae9c6b 7
ewoud 9:07189a75e979 8 void setGoFlag(){
ewoud 9:07189a75e979 9 goFlag=true;
ewoud 9:07189a75e979 10 }
aberk 0:9bca35ae9c6b 11
ewoud 7:e14e28d8cae3 12 int main() {
ewoud 7:e14e28d8cae3 13
ewoud 9:07189a75e979 14 // initialize (defined in inits.h)
ewoud 9:07189a75e979 15 initMotors();
ewoud 9:07189a75e979 16 initPIDs();
ewoud 9:07189a75e979 17
ewoud 9:07189a75e979 18 motorControlTicker.attach(&setGoFlag, RATE);
ewoud 9:07189a75e979 19
ewoud 8:55ca92c0e39d 20 endTimer.start(); //Run for 100 seconds.
ewoud 4:be465e9a12cb 21 while (endTimer.read() < 100){
ewoud 9:07189a75e979 22 if (goFlag==true){
ewoud 9:07189a75e979 23 // get 'emg' signal
ewoud 9:07189a75e979 24 request_pos = pot1.read();
ewoud 9:07189a75e979 25 request_neg = pot2.read();
ewoud 9:07189a75e979 26
ewoud 9:07189a75e979 27 // determine if forward or backward signal is stronger and set reference
ewoud 9:07189a75e979 28 if (request_pos > request_neg){
ewoud 9:07189a75e979 29 request = request_pos;
ewoud 9:07189a75e979 30 }
ewoud 9:07189a75e979 31 else {
ewoud 9:07189a75e979 32 request = -request_neg;
ewoud 9:07189a75e979 33 }
ewoud 9:07189a75e979 34 leftController.setSetPoint(request);
ewoud 9:07189a75e979 35
ewoud 9:07189a75e979 36 // Velocity calculation
ewoud 9:07189a75e979 37 leftPulses = leftQei.getPulses();
ewoud 9:07189a75e979 38 leftVelocity = ((float)(leftPulses - leftPrevPulses))/ RATE;
ewoud 9:07189a75e979 39 leftVelocity = leftVelocity/30000; // scale to 0 - 1, max velocity = 30000
ewoud 9:07189a75e979 40 leftPrevPulses = leftPulses;
ewoud 9:07189a75e979 41
ewoud 9:07189a75e979 42 // PID control
ewoud 9:07189a75e979 43 leftController.setProcessValue(leftVelocity);
ewoud 9:07189a75e979 44 leftPwmDuty = leftController.compute();
ewoud 9:07189a75e979 45 if (leftPwmDuty < 0){
ewoud 9:07189a75e979 46 leftDirection = 0;
ewoud 9:07189a75e979 47 leftMotor = -leftPwmDuty;
ewoud 9:07189a75e979 48 }
ewoud 9:07189a75e979 49 else {
ewoud 9:07189a75e979 50 leftDirection = 1;
ewoud 9:07189a75e979 51 leftMotor = leftPwmDuty;
ewoud 9:07189a75e979 52 }
ewoud 9:07189a75e979 53
ewoud 9:07189a75e979 54 // testing the right motor
ewoud 9:07189a75e979 55 rightMotor = leftMotor;
ewoud 9:07189a75e979 56 rightDirection=leftDirection;
ewoud 9:07189a75e979 57
ewoud 9:07189a75e979 58 // User feadback
ewoud 9:07189a75e979 59 scope.set(0, request);
ewoud 9:07189a75e979 60 scope.set(1, leftPwmDuty);
ewoud 9:07189a75e979 61 scope.set(2, leftVelocity);
ewoud 9:07189a75e979 62 scope.send();
ewoud 9:07189a75e979 63 pc.printf("pot2: %f, request: %f, lefVelocity: %f, output: %f \n\r",pot2.read(),request,leftVelocity,leftPwmDuty);
ewoud 8:55ca92c0e39d 64
ewoud 9:07189a75e979 65 goFlag=false;
ewoud 7:e14e28d8cae3 66 }
aberk 0:9bca35ae9c6b 67 }
aberk 0:9bca35ae9c6b 68
aberk 0:9bca35ae9c6b 69 //Stop motors.
ewoud 2:b2ccd9f044bb 70 leftMotor = 0;
aberk 0:9bca35ae9c6b 71
aberk 0:9bca35ae9c6b 72 //Close results file.
ewoud 2:b2ccd9f044bb 73 //fclose(fp);
aberk 0:9bca35ae9c6b 74
aberk 0:9bca35ae9c6b 75 }