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@6:ae2ce89dd695, 2015-10-20 (annotated)

Committer:
ewoud
Date:
Tue Oct 20 07:52:45 2015 +0000
Revision:
6:ae2ce89dd695
Parent:
3:70f78cfc0f25
Child:
8:b0971033dc41
idea for init position

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ewoud 3:70f78cfc0f25 1
ewoud 2:8f9b6c1e89cf 2 #define RATE 0.005f
ewoud 0:ca94aa9bf823 3 #define Kc 0.30
ewoud 0:ca94aa9bf823 4 #define Ti 0.00
ewoud 0:ca94aa9bf823 5 #define Td 0.0
ewoud 0:ca94aa9bf823 6
ewoud 3:70f78cfc0f25 7 ///////////////////////
ewoud 3:70f78cfc0f25 8 /// pin layout ////////
ewoud 3:70f78cfc0f25 9 ///////////////////////
ewoud 3:70f78cfc0f25 10
ewoud 0:ca94aa9bf823 11 Serial pc(USBTX, USBRX);
ewoud 3:70f78cfc0f25 12
ewoud 0:ca94aa9bf823 13 // Left motor.
ewoud 0:ca94aa9bf823 14 PwmOut leftMotor(D5);
ewoud 0:ca94aa9bf823 15 DigitalOut leftDirection(D4);
ewoud 0:ca94aa9bf823 16 QEI leftQei(D12, D13, NC, 624);
ewoud 0:ca94aa9bf823 17 PID leftController(Kc, Ti, Td, RATE);
ewoud 0:ca94aa9bf823 18
ewoud 0:ca94aa9bf823 19 // Right motor
ewoud 0:ca94aa9bf823 20 PwmOut rightMotor(D6);
ewoud 0:ca94aa9bf823 21 DigitalOut rightDirection(D7);
ewoud 0:ca94aa9bf823 22 QEI rightQei(D11, D10, NC, 624);
ewoud 0:ca94aa9bf823 23 PID rightController(Kc, Ti, Td, RATE);
ewoud 0:ca94aa9bf823 24
ewoud 0:ca94aa9bf823 25 // edge leads
ewoud 3:70f78cfc0f25 26 DigitalIn edgeleft(PTC5);
ewoud 3:70f78cfc0f25 27 DigitalIn edgeright(PTC7);
ewoud 3:70f78cfc0f25 28 DigitalIn edgetop(PTC0);
ewoud 3:70f78cfc0f25 29 DigitalIn edgebottom(PTC9);
ewoud 0:ca94aa9bf823 30
ewoud 3:70f78cfc0f25 31 // emg input
ewoud 0:ca94aa9bf823 32 AnalogIn pot1(A0);
ewoud 0:ca94aa9bf823 33 AnalogIn pot2(A1);
ewoud 3:70f78cfc0f25 34
ewoud 3:70f78cfc0f25 35 // user interface
ewoud 3:70f78cfc0f25 36 DigitalOut outofboundsled(LED1);
ewoud 3:70f78cfc0f25 37 InterruptIn startButton(D3);
ewoud 3:70f78cfc0f25 38 InterruptIn stopButton(D2);
ewoud 6:ae2ce89dd695 39 InterruptIn initpositionButton(PTC6);
ewoud 3:70f78cfc0f25 40 HIDScope scope(5);
ewoud 3:70f78cfc0f25 41
ewoud 3:70f78cfc0f25 42 ////////////////////////////
ewoud 3:70f78cfc0f25 43 //////// Timers ////////////
ewoud 3:70f78cfc0f25 44 ////////////////////////////
ewoud 3:70f78cfc0f25 45 Ticker scopeTimer;
ewoud 0:ca94aa9bf823 46 Timer endTimer;
ewoud 0:ca94aa9bf823 47 Ticker motorControlTicker;
ewoud 0:ca94aa9bf823 48 bool goFlag=false;
ewoud 0:ca94aa9bf823 49 bool systemOn=false;
ewoud 6:ae2ce89dd695 50 bool initposition_go=false;
ewoud 3:70f78cfc0f25 51
ewoud 3:70f78cfc0f25 52
ewoud 3:70f78cfc0f25 53 ////////////////////////////
ewoud 3:70f78cfc0f25 54 ///////// Variables ////////
ewoud 3:70f78cfc0f25 55 ////////////////////////////
ewoud 3:70f78cfc0f25 56
ewoud 3:70f78cfc0f25 57
ewoud 3:70f78cfc0f25 58 // Working variables for motors
ewoud 0:ca94aa9bf823 59 volatile int leftPulses = 0;
ewoud 0:ca94aa9bf823 60 volatile float leftAngle=0;
ewoud 0:ca94aa9bf823 61 volatile int leftPrevPulses = 0;
ewoud 0:ca94aa9bf823 62 volatile float leftPwmDuty = 0.0;
ewoud 0:ca94aa9bf823 63 volatile float leftVelocity = 0.0;
ewoud 0:ca94aa9bf823 64 float leftRequest=0;
ewoud 0:ca94aa9bf823 65
ewoud 0:ca94aa9bf823 66 volatile int rightPulses = 0;
ewoud 0:ca94aa9bf823 67 volatile int rightPrevPulses = 0;
ewoud 0:ca94aa9bf823 68 volatile float rightAngle =0;
ewoud 0:ca94aa9bf823 69 volatile float rightPwmDuty = 0.0;
ewoud 0:ca94aa9bf823 70 volatile float rightVelocity = 0.0;
ewoud 0:ca94aa9bf823 71 float rightRequest=0;
ewoud 0:ca94aa9bf823 72
ewoud 3:70f78cfc0f25 73 // working vars for calculation
ewoud 3:70f78cfc0f25 74 float horrequest;
ewoud 3:70f78cfc0f25 75 float verrequest;
ewoud 3:70f78cfc0f25 76 float grenslaag=0.3;
ewoud 3:70f78cfc0f25 77 float grenshoog=0.7;
ewoud 0:ca94aa9bf823 78 float round=27720;
ewoud 3:70f78cfc0f25 79 float maxspeed=1; //cm/sec
ewoud 0:ca94aa9bf823 80 float currentX;
ewoud 0:ca94aa9bf823 81 float currentY;
ewoud 0:ca94aa9bf823 82 float l=10;
ewoud 0:ca94aa9bf823 83 float toX;
ewoud 0:ca94aa9bf823 84 float toY;
ewoud 0:ca94aa9bf823 85 float toLeftAngle;
ewoud 0:ca94aa9bf823 86 float toRightAngle;
ewoud 0:ca94aa9bf823 87 float M_PI=3.14159265359;
ewoud 0:ca94aa9bf823 88
ewoud 3:70f78cfc0f25 89 //////////////////////////////////
ewoud 3:70f78cfc0f25 90 ////// initialize functions //////
ewoud 3:70f78cfc0f25 91 //////////////////////////////////
ewoud 0:ca94aa9bf823 92
ewoud 0:ca94aa9bf823 93 void initMotors(){
ewoud 0:ca94aa9bf823 94 //Initialization of motor
ewoud 0:ca94aa9bf823 95 leftMotor.period_us(50);
ewoud 0:ca94aa9bf823 96 leftMotor = 0.0;
ewoud 0:ca94aa9bf823 97 leftDirection = 1;
ewoud 0:ca94aa9bf823 98
ewoud 0:ca94aa9bf823 99 rightMotor.period_us(50);
ewoud 0:ca94aa9bf823 100 rightMotor = 0.0;
ewoud 0:ca94aa9bf823 101 rightDirection = 1;
ewoud 0:ca94aa9bf823 102
ewoud 0:ca94aa9bf823 103 }
ewoud 0:ca94aa9bf823 104
ewoud 0:ca94aa9bf823 105 void initPIDs(){
ewoud 0:ca94aa9bf823 106 //Initialization of PID controller
ewoud 3:70f78cfc0f25 107 leftController.setInputLimits(-90.0, 90.0);
ewoud 0:ca94aa9bf823 108 leftController.setOutputLimits(-1.0, 1.0);
ewoud 0:ca94aa9bf823 109 leftController.setBias(0);
ewoud 0:ca94aa9bf823 110 leftController.setDeadzone(-0.4, 0.4);
ewoud 0:ca94aa9bf823 111 leftController.setMode(AUTO_MODE);
ewoud 0:ca94aa9bf823 112
ewoud 3:70f78cfc0f25 113 rightController.setInputLimits(-90.0, 90.0);
ewoud 0:ca94aa9bf823 114 rightController.setOutputLimits(-1.0, 1.0);
ewoud 0:ca94aa9bf823 115 rightController.setBias(0);
ewoud 0:ca94aa9bf823 116 rightController.setDeadzone(-0.4, 0.4);
ewoud 0:ca94aa9bf823 117 rightController.setMode(AUTO_MODE);
ewoud 0:ca94aa9bf823 118 }