Arthur Oliveira / PTC3471

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.

PTC3471.cpp@1:00b8f71cfe7e, 2017-08-07 (annotated)

Committer:
ArthurCBdO
Date:
Mon Aug 07 22:32:23 2017 +0000
Revision:
1:00b8f71cfe7e
Parent:
0:f2bc43be0fc7 
Template Zerado Funcionando de verdade

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ArthurCBdO 0:f2bc43be0fc7 1 #include "PTC3471.h"
ArthurCBdO 0:f2bc43be0fc7 2 #include "mbed.h"
ArthurCBdO 0:f2bc43be0fc7 3 #include "QEI.h"
ArthurCBdO 0:f2bc43be0fc7 4 #include "USBSerial.h"
ArthurCBdO 0:f2bc43be0fc7 5
ArthurCBdO 0:f2bc43be0fc7 6 DigitalOut Ctrl_Status(LED1);
ArthurCBdO 0:f2bc43be0fc7 7
ArthurCBdO 0:f2bc43be0fc7 8 QEI * Ctrl_Encoder_Motor;
ArthurCBdO 1:00b8f71cfe7e 9 Ticker * Controler_Interrupt;
ArthurCBdO 0:f2bc43be0fc7 10
ArthurCBdO 0:f2bc43be0fc7 11 DigitalOut Ctrl_Direita(PTC1);
ArthurCBdO 0:f2bc43be0fc7 12 DigitalOut Ctrl_Esquerda(PTD6);
ArthurCBdO 0:f2bc43be0fc7 13 PwmOut Ctrl_Motor(PTD5);
ArthurCBdO 0:f2bc43be0fc7 14
ArthurCBdO 0:f2bc43be0fc7 15 Ticker Ctrl_StatusOk;
ArthurCBdO 0:f2bc43be0fc7 16 Ticker Ctrl_Trava;
ArthurCBdO 0:f2bc43be0fc7 17
ArthurCBdO 1:00b8f71cfe7e 18 double Ctrl_Phi = 0;
ArthurCBdO 1:00b8f71cfe7e 19 double Ctrl_Phi1 = 0;
ArthurCBdO 1:00b8f71cfe7e 20 double Ctrl_dPhi = 0;
ArthurCBdO 0:f2bc43be0fc7 21
ArthurCBdO 0:f2bc43be0fc7 22 void Ctrl_LerAngulos(void){
ArthurCBdO 0:f2bc43be0fc7 23 Ctrl_Phi1 = Ctrl_Phi;
ArthurCBdO 0:f2bc43be0fc7 24 Ctrl_Phi = pi*Ctrl_Encoder_Motor->getPulses()/600.0;
ArthurCBdO 0:f2bc43be0fc7 25 Ctrl_dPhi = (Ctrl_Phi-Ctrl_Phi1);
ArthurCBdO 0:f2bc43be0fc7 26 }
ArthurCBdO 0:f2bc43be0fc7 27
ArthurCBdO 0:f2bc43be0fc7 28 void Ctrl_Travar(void){
ArthurCBdO 0:f2bc43be0fc7 29 Ctrl_LerAngulos();
ArthurCBdO 0:f2bc43be0fc7 30
ArthurCBdO 0:f2bc43be0fc7 31 if(Ctrl_dPhi>=0.01){
ArthurCBdO 1:00b8f71cfe7e 32 Ctrl_Motor = min(2*Ctrl_dPhi, 0.7);
ArthurCBdO 0:f2bc43be0fc7 33 Ctrl_Direita = 1;
ArthurCBdO 0:f2bc43be0fc7 34 Ctrl_Esquerda = 0;
ArthurCBdO 0:f2bc43be0fc7 35 }
ArthurCBdO 0:f2bc43be0fc7 36 else if(Ctrl_dPhi<=-0.01){
ArthurCBdO 1:00b8f71cfe7e 37 Ctrl_Motor = min(-2*Ctrl_dPhi, 0.7);
ArthurCBdO 0:f2bc43be0fc7 38 Ctrl_Direita = 0;
ArthurCBdO 0:f2bc43be0fc7 39 Ctrl_Esquerda = 1;
ArthurCBdO 0:f2bc43be0fc7 40 }
ArthurCBdO 1:00b8f71cfe7e 41 else{
ArthurCBdO 1:00b8f71cfe7e 42 Ctrl_Direita=1;
ArthurCBdO 1:00b8f71cfe7e 43 Ctrl_Esquerda=1;
ArthurCBdO 1:00b8f71cfe7e 44 }
ArthurCBdO 0:f2bc43be0fc7 45
ArthurCBdO 0:f2bc43be0fc7 46 if(Ctrl_Status==0){
ArthurCBdO 0:f2bc43be0fc7 47 Ctrl_Status = 1;
ArthurCBdO 0:f2bc43be0fc7 48 }
ArthurCBdO 0:f2bc43be0fc7 49 else{
ArthurCBdO 0:f2bc43be0fc7 50 Ctrl_Status = 0;
ArthurCBdO 0:f2bc43be0fc7 51 }
ArthurCBdO 0:f2bc43be0fc7 52 }
ArthurCBdO 0:f2bc43be0fc7 53
ArthurCBdO 0:f2bc43be0fc7 54 void Ctrl_ChecarTrava(void){
ArthurCBdO 1:00b8f71cfe7e 55 Ctrl_LerAngulos();
ArthurCBdO 0:f2bc43be0fc7 56 if(Ctrl_Phi>pi || Ctrl_Phi<-pi){
ArthurCBdO 0:f2bc43be0fc7 57 Ctrl_StatusOk.detach();
ArthurCBdO 0:f2bc43be0fc7 58 Ctrl_Trava.detach();
ArthurCBdO 1:00b8f71cfe7e 59 Controler_Interrupt->detach();
ArthurCBdO 1:00b8f71cfe7e 60 Ctrl_Trava.attach(&Ctrl_Travar, Ctrl_Ts);
ArthurCBdO 0:f2bc43be0fc7 61 }
ArthurCBdO 0:f2bc43be0fc7 62 }
ArthurCBdO 0:f2bc43be0fc7 63
ArthurCBdO 0:f2bc43be0fc7 64 void Ctrl_BlinkOk(void){
ArthurCBdO 0:f2bc43be0fc7 65 Ctrl_Status = !Ctrl_Status;
ArthurCBdO 0:f2bc43be0fc7 66 }
ArthurCBdO 0:f2bc43be0fc7 67
ArthurCBdO 1:00b8f71cfe7e 68 void Protecao_Init(QEI* Encoder_Motor, Ticker* Control_Interrupt){
ArthurCBdO 0:f2bc43be0fc7 69 Ctrl_Encoder_Motor = Encoder_Motor;
ArthurCBdO 1:00b8f71cfe7e 70 Controler_Interrupt = Control_Interrupt;
ArthurCBdO 0:f2bc43be0fc7 71 Ctrl_Status = 1;
ArthurCBdO 0:f2bc43be0fc7 72 Ctrl_StatusOk.attach(&Ctrl_BlinkOk, 1);
ArthurCBdO 0:f2bc43be0fc7 73 Ctrl_Trava.attach(&Ctrl_ChecarTrava, Ctrl_Ts);
ArthurCBdO 0:f2bc43be0fc7 74
ArthurCBdO 0:f2bc43be0fc7 75 Ctrl_Motor = 0.0;
ArthurCBdO 0:f2bc43be0fc7 76 Ctrl_Direita = 0;
ArthurCBdO 0:f2bc43be0fc7 77 Ctrl_Esquerda = 1;
ArthurCBdO 0:f2bc43be0fc7 78 }