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@10:819fb5288aa0, 2015-10-27 (annotated)

Committer:
ewoud
Date:
Tue Oct 27 17:18:17 2015 +0000
Revision:
10:819fb5288aa0
Parent:
9:3e19a344c025
Child:
11:c5042e19a096
working very smooth. fixed float->int problem, correct lcd working

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 8:b0971033dc41 12 TextLCD lcd(D9, D14, D15, PTB19, PTB18, A4); // rs, e, d4 to d7
ewoud 3:70f78cfc0f25 13
ewoud 8:b0971033dc41 14 // Left motor. (motor connection 1)
ewoud 8:b0971033dc41 15 PwmOut leftMotor(D5);
ewoud 0:ca94aa9bf823 16 DigitalOut leftDirection(D4);
ewoud 0:ca94aa9bf823 17 QEI leftQei(D12, D13, NC, 624);
ewoud 0:ca94aa9bf823 18 PID leftController(Kc, Ti, Td, RATE);
ewoud 0:ca94aa9bf823 19
ewoud 8:b0971033dc41 20 // Right motor (motor connection 2)
ewoud 0:ca94aa9bf823 21 PwmOut rightMotor(D6);
ewoud 0:ca94aa9bf823 22 DigitalOut rightDirection(D7);
ewoud 0:ca94aa9bf823 23 QEI rightQei(D11, D10, NC, 624);
ewoud 0:ca94aa9bf823 24 PID rightController(Kc, Ti, Td, RATE);
ewoud 0:ca94aa9bf823 25
ewoud 0:ca94aa9bf823 26 // edge leads
ewoud 3:70f78cfc0f25 27 DigitalIn edgeleft(PTC5);
ewoud 3:70f78cfc0f25 28 DigitalIn edgeright(PTC7);
ewoud 3:70f78cfc0f25 29 DigitalIn edgetop(PTC0);
ewoud 3:70f78cfc0f25 30 DigitalIn edgebottom(PTC9);
ewoud 10:819fb5288aa0 31 DigitalIn edgeStart(PTC8);
ewoud 10:819fb5288aa0 32 DigitalIn edgeFinish(PTC1);
ewoud 0:ca94aa9bf823 33
ewoud 10:819fb5288aa0 34 // pot input
ewoud 0:ca94aa9bf823 35 AnalogIn pot1(A0);
ewoud 0:ca94aa9bf823 36 AnalogIn pot2(A1);
ewoud 3:70f78cfc0f25 37
ewoud 3:70f78cfc0f25 38 // user interface
ewoud 10:819fb5288aa0 39 DigitalOut statusled(LED_GREEN);
ewoud 3:70f78cfc0f25 40 InterruptIn startButton(D3);
ewoud 10:819fb5288aa0 41 bool calibrated=false; // bool to determine if button press has to start calibration or the game
ewoud 3:70f78cfc0f25 42 InterruptIn stopButton(D2);
ewoud 6:ae2ce89dd695 43 InterruptIn initpositionButton(PTC6);
ewoud 3:70f78cfc0f25 44 HIDScope scope(5);
ewoud 10:819fb5288aa0 45 bool controlbypc=false;
ewoud 3:70f78cfc0f25 46
ewoud 3:70f78cfc0f25 47 ////////////////////////////
ewoud 3:70f78cfc0f25 48 //////// Timers ////////////
ewoud 3:70f78cfc0f25 49 ////////////////////////////
ewoud 3:70f78cfc0f25 50 Ticker scopeTimer;
ewoud 9:3e19a344c025 51 Timer playTimer;
ewoud 0:ca94aa9bf823 52 Ticker motorControlTicker;
ewoud 9:3e19a344c025 53 Ticker lcdTicker;
ewoud 9:3e19a344c025 54 bool lcdGoFlag=false;
ewoud 0:ca94aa9bf823 55 bool goFlag=false;
ewoud 0:ca94aa9bf823 56 bool systemOn=false;
ewoud 6:ae2ce89dd695 57 bool initposition_go=false;
ewoud 3:70f78cfc0f25 58
ewoud 3:70f78cfc0f25 59
ewoud 3:70f78cfc0f25 60 ////////////////////////////
ewoud 3:70f78cfc0f25 61 ///////// Variables ////////
ewoud 3:70f78cfc0f25 62 ////////////////////////////
ewoud 3:70f78cfc0f25 63
ewoud 3:70f78cfc0f25 64
ewoud 3:70f78cfc0f25 65 // Working variables for motors
ewoud 10:819fb5288aa0 66 volatile float leftPulses = 0;
ewoud 0:ca94aa9bf823 67 volatile float leftAngle=0;
ewoud 10:819fb5288aa0 68 volatile float leftPrevPulses = 0;
ewoud 0:ca94aa9bf823 69 volatile float leftPwmDuty = 0.0;
ewoud 0:ca94aa9bf823 70 volatile float leftVelocity = 0.0;
ewoud 0:ca94aa9bf823 71 float leftRequest=0;
ewoud 0:ca94aa9bf823 72
ewoud 10:819fb5288aa0 73 volatile float rightPulses = 0;
ewoud 10:819fb5288aa0 74 volatile float rightPrevPulses = 0;
ewoud 0:ca94aa9bf823 75 volatile float rightAngle =0;
ewoud 0:ca94aa9bf823 76 volatile float rightPwmDuty = 0.0;
ewoud 0:ca94aa9bf823 77 volatile float rightVelocity = 0.0;
ewoud 0:ca94aa9bf823 78 float rightRequest=0;
ewoud 0:ca94aa9bf823 79
ewoud 3:70f78cfc0f25 80 // working vars for calculation
ewoud 3:70f78cfc0f25 81 float horrequest;
ewoud 3:70f78cfc0f25 82 float verrequest;
ewoud 3:70f78cfc0f25 83 float grenslaag=0.3;
ewoud 3:70f78cfc0f25 84 float grenshoog=0.7;
ewoud 10:819fb5288aa0 85 float round=4200.0*6.6*6.6;
ewoud 9:3e19a344c025 86 float maxspeed=1; //cm/sec
ewoud 0:ca94aa9bf823 87 float currentX;
ewoud 0:ca94aa9bf823 88 float currentY;
ewoud 8:b0971033dc41 89 float l=57;
ewoud 0:ca94aa9bf823 90 float toX;
ewoud 0:ca94aa9bf823 91 float toY;
ewoud 0:ca94aa9bf823 92 float toLeftAngle;
ewoud 0:ca94aa9bf823 93 float toRightAngle;
ewoud 0:ca94aa9bf823 94 float M_PI=3.14159265359;
ewoud 9:3e19a344c025 95 string text;
ewoud 0:ca94aa9bf823 96
ewoud 3:70f78cfc0f25 97 //////////////////////////////////
ewoud 3:70f78cfc0f25 98 ////// initialize functions //////
ewoud 3:70f78cfc0f25 99 //////////////////////////////////
ewoud 0:ca94aa9bf823 100
ewoud 0:ca94aa9bf823 101 void initMotors(){
ewoud 0:ca94aa9bf823 102 //Initialization of motor
ewoud 0:ca94aa9bf823 103 leftMotor.period_us(50);
ewoud 0:ca94aa9bf823 104 leftMotor = 0.0;
ewoud 0:ca94aa9bf823 105 leftDirection = 1;
ewoud 0:ca94aa9bf823 106
ewoud 0:ca94aa9bf823 107 rightMotor.period_us(50);
ewoud 0:ca94aa9bf823 108 rightMotor = 0.0;
ewoud 0:ca94aa9bf823 109 rightDirection = 1;
ewoud 0:ca94aa9bf823 110
ewoud 0:ca94aa9bf823 111 }
ewoud 0:ca94aa9bf823 112
ewoud 0:ca94aa9bf823 113 void initPIDs(){
ewoud 0:ca94aa9bf823 114 //Initialization of PID controller
ewoud 3:70f78cfc0f25 115 leftController.setInputLimits(-90.0, 90.0);
ewoud 0:ca94aa9bf823 116 leftController.setOutputLimits(-1.0, 1.0);
ewoud 0:ca94aa9bf823 117 leftController.setBias(0);
ewoud 0:ca94aa9bf823 118 leftController.setDeadzone(-0.4, 0.4);
ewoud 0:ca94aa9bf823 119 leftController.setMode(AUTO_MODE);
ewoud 0:ca94aa9bf823 120
ewoud 3:70f78cfc0f25 121 rightController.setInputLimits(-90.0, 90.0);
ewoud 0:ca94aa9bf823 122 rightController.setOutputLimits(-1.0, 1.0);
ewoud 0:ca94aa9bf823 123 rightController.setBias(0);
ewoud 0:ca94aa9bf823 124 rightController.setDeadzone(-0.4, 0.4);
ewoud 0:ca94aa9bf823 125 rightController.setMode(AUTO_MODE);
ewoud 0:ca94aa9bf823 126 }