Numero Uno / Mbed 2 deprecated TheProgram

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   EMG HIDScope PID QEI mbed TextLCD

inits.h@0:ca94aa9bf823, 2015-10-13 (annotated)

Committer:
ewoud
Date:
Tue Oct 13 17:46:45 2015 +0000
Revision:
0:ca94aa9bf823
Child:
2:8f9b6c1e89cf
working emg + xy to rotation + p controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 0:ca94aa9bf823 1 //****************************************************************************/
ewoud 0:ca94aa9bf823 2 // Defines
ewoud 0:ca94aa9bf823 3 //****************************************************************************/
ewoud 0:ca94aa9bf823 4 #define RATE 0.005
ewoud 0:ca94aa9bf823 5 #define Kc 0.30
ewoud 0:ca94aa9bf823 6 #define Ti 0.00
ewoud 0:ca94aa9bf823 7 #define Td 0.0
ewoud 0:ca94aa9bf823 8
ewoud 0:ca94aa9bf823 9 //****************************************************************************/
ewoud 0:ca94aa9bf823 10 // Globals
ewoud 0:ca94aa9bf823 11 //****************************************************************************/
ewoud 0:ca94aa9bf823 12 Serial pc(USBTX, USBRX);
ewoud 0:ca94aa9bf823 13 HIDScope scope(5); // Instantize a 2-channel HIDScope object
ewoud 0:ca94aa9bf823 14 Ticker scopeTimer; // Instantize the timer for sending data to the PC
ewoud 0:ca94aa9bf823 15 InterruptIn startButton(D3);
ewoud 0:ca94aa9bf823 16 InterruptIn stopButton(D2);
ewoud 0:ca94aa9bf823 17 //--------
ewoud 0:ca94aa9bf823 18 // Motors
ewoud 0:ca94aa9bf823 19 //--------
ewoud 0:ca94aa9bf823 20 // Left motor.
ewoud 0:ca94aa9bf823 21 PwmOut leftMotor(D5);
ewoud 0:ca94aa9bf823 22 DigitalOut leftDirection(D4);
ewoud 0:ca94aa9bf823 23 QEI leftQei(D12, D13, NC, 624);
ewoud 0:ca94aa9bf823 24 PID leftController(Kc, Ti, Td, RATE);
ewoud 0:ca94aa9bf823 25
ewoud 0:ca94aa9bf823 26 // Right motor
ewoud 0:ca94aa9bf823 27 PwmOut rightMotor(D6);
ewoud 0:ca94aa9bf823 28 DigitalOut rightDirection(D7);
ewoud 0:ca94aa9bf823 29 QEI rightQei(D11, D10, NC, 624);
ewoud 0:ca94aa9bf823 30 PID rightController(Kc, Ti, Td, RATE);
ewoud 0:ca94aa9bf823 31
ewoud 0:ca94aa9bf823 32 // edge leads
ewoud 0:ca94aa9bf823 33 DigitalOut outofboundsled(LED1);
ewoud 0:ca94aa9bf823 34 DigitalIn edgeleft(D0);
ewoud 0:ca94aa9bf823 35 DigitalIn edgeright(D1);
ewoud 0:ca94aa9bf823 36
ewoud 0:ca94aa9bf823 37
ewoud 0:ca94aa9bf823 38 // EMG input
ewoud 0:ca94aa9bf823 39 AnalogIn pot1(A0);
ewoud 0:ca94aa9bf823 40 AnalogIn pot2(A1);
ewoud 0:ca94aa9bf823 41 biquadFilter leftVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
ewoud 0:ca94aa9bf823 42 biquadFilter rightVelocityfilter((double) -1.561018075800718, (double) 0.641351538057563, (double) 0.020083365564211, (double) 0.040166731128423, (double) 0.020083365564211);
ewoud 0:ca94aa9bf823 43 // Timers
ewoud 0:ca94aa9bf823 44 Timer endTimer;
ewoud 0:ca94aa9bf823 45 Ticker motorControlTicker;
ewoud 0:ca94aa9bf823 46 bool goFlag=false;
ewoud 0:ca94aa9bf823 47 bool systemOn=false;
ewoud 0:ca94aa9bf823 48 // Working variables.
ewoud 0:ca94aa9bf823 49 volatile int leftPulses = 0;
ewoud 0:ca94aa9bf823 50 volatile float leftAngle=0;
ewoud 0:ca94aa9bf823 51 volatile int leftPrevPulses = 0;
ewoud 0:ca94aa9bf823 52 volatile float leftPwmDuty = 0.0;
ewoud 0:ca94aa9bf823 53 volatile float leftPwmDutyPrev = 0.0;
ewoud 0:ca94aa9bf823 54 volatile float leftVelocity = 0.0;
ewoud 0:ca94aa9bf823 55 float leftRequest=0;
ewoud 0:ca94aa9bf823 56
ewoud 0:ca94aa9bf823 57 volatile int rightPulses = 0;
ewoud 0:ca94aa9bf823 58 volatile int rightPrevPulses = 0;
ewoud 0:ca94aa9bf823 59 volatile float rightAngle =0;
ewoud 0:ca94aa9bf823 60 volatile float rightPwmDuty = 0.0;
ewoud 0:ca94aa9bf823 61 volatile float rightPwmDutyPrev = 0.0;
ewoud 0:ca94aa9bf823 62 volatile float rightVelocity = 0.0;
ewoud 0:ca94aa9bf823 63 float rightRequest=0;
ewoud 0:ca94aa9bf823 64
ewoud 0:ca94aa9bf823 65 float request;
ewoud 0:ca94aa9bf823 66 float request_pos;
ewoud 0:ca94aa9bf823 67 float request_neg;
ewoud 0:ca94aa9bf823 68 float round=27720;
ewoud 0:ca94aa9bf823 69 float maxspeed=10; //cm/sec
ewoud 0:ca94aa9bf823 70 float currentX;
ewoud 0:ca94aa9bf823 71 float currentY;
ewoud 0:ca94aa9bf823 72 float l=10;
ewoud 0:ca94aa9bf823 73 float toX;
ewoud 0:ca94aa9bf823 74 float toY;
ewoud 0:ca94aa9bf823 75 float toLeftAngle;
ewoud 0:ca94aa9bf823 76 float toRightAngle;
ewoud 0:ca94aa9bf823 77 float leftDeltaAngle;
ewoud 0:ca94aa9bf823 78 float rightDeltaAngle;
ewoud 0:ca94aa9bf823 79 float M_PI=3.14159265359;
ewoud 0:ca94aa9bf823 80
ewoud 0:ca94aa9bf823 81
ewoud 0:ca94aa9bf823 82 void initMotors(){
ewoud 0:ca94aa9bf823 83 //Initialization of motor
ewoud 0:ca94aa9bf823 84 leftMotor.period_us(50);
ewoud 0:ca94aa9bf823 85 leftMotor = 0.0;
ewoud 0:ca94aa9bf823 86 leftDirection = 1;
ewoud 0:ca94aa9bf823 87
ewoud 0:ca94aa9bf823 88 rightMotor.period_us(50);
ewoud 0:ca94aa9bf823 89 rightMotor = 0.0;
ewoud 0:ca94aa9bf823 90 rightDirection = 1;
ewoud 0:ca94aa9bf823 91
ewoud 0:ca94aa9bf823 92 }
ewoud 0:ca94aa9bf823 93
ewoud 0:ca94aa9bf823 94 void initPIDs(){
ewoud 0:ca94aa9bf823 95 //Initialization of PID controller
ewoud 0:ca94aa9bf823 96 leftController.setInputLimits(-180.0, 180.0);
ewoud 0:ca94aa9bf823 97 leftController.setOutputLimits(-1.0, 1.0);
ewoud 0:ca94aa9bf823 98 leftController.setBias(0);
ewoud 0:ca94aa9bf823 99 leftController.setDeadzone(-0.4, 0.4);
ewoud 0:ca94aa9bf823 100 leftController.setMode(AUTO_MODE);
ewoud 0:ca94aa9bf823 101
ewoud 0:ca94aa9bf823 102 rightController.setInputLimits(-180.0, 180.0);
ewoud 0:ca94aa9bf823 103 rightController.setOutputLimits(-1.0, 1.0);
ewoud 0:ca94aa9bf823 104 rightController.setBias(0);
ewoud 0:ca94aa9bf823 105 rightController.setDeadzone(-0.4, 0.4);
ewoud 0:ca94aa9bf823 106 rightController.setMode(AUTO_MODE);
ewoud 0:ca94aa9bf823 107 }