Group21 ESP / Mbed 2 deprecated TD1

y

Dependencies:   mbed C12832 QEI

main.cpp@0:d33abe99b8aa, 2020-02-18 (annotated)

Committer:
louis_mynott
Date:
Tue Feb 18 20:13:02 2020 +0000
Revision:
0:d33abe99b8aa
re

Who changed what in which revision?

UserRevisionLine numberNew contents of line
louis_mynott 0:d33abe99b8aa 1 #include "mbed.h"
louis_mynott 0:d33abe99b8aa 2 #include "QEI.h"
louis_mynott 0:d33abe99b8aa 3 #include "C12832.h"
louis_mynott 0:d33abe99b8aa 4
louis_mynott 0:d33abe99b8aa 5
louis_mynott 0:d33abe99b8aa 6 C12832 lcd(D11, D13, D12, D7, D10);
louis_mynott 0:d33abe99b8aa 7
louis_mynott 0:d33abe99b8aa 8 class Motor {
louis_mynott 0:d33abe99b8aa 9 private:
louis_mynott 0:d33abe99b8aa 10 PwmOut pwm;
louis_mynott 0:d33abe99b8aa 11 DigitalOut direction, pole;
louis_mynott 0:d33abe99b8aa 12 float pwmWidth, offset;
louis_mynott 0:d33abe99b8aa 13 int high;
louis_mynott 0:d33abe99b8aa 14 public:
louis_mynott 0:d33abe99b8aa 15 Motor(PinName pwmPin, PinName directionPin, PinName polePin, int ofst, int h): pwm(pwmPin), direction(directionPin), pole(polePin), offset(ofst), high(h){
louis_mynott 0:d33abe99b8aa 16 pwm.period_us(5);
louis_mynott 0:d33abe99b8aa 17 pwmWidth = 0.5+(offset/100.0);
louis_mynott 0:d33abe99b8aa 18 pole = 0;
louis_mynott 0:d33abe99b8aa 19 }
louis_mynott 0:d33abe99b8aa 20 void PWM(double transC, double angC, int drctn){ // PWM(speed error, angle error, direction)
louis_mynott 0:d33abe99b8aa 21
louis_mynott 0:d33abe99b8aa 22 if (transC == 0 || angC == 0){
louis_mynott 0:d33abe99b8aa 23 pwm.write(1-high);
louis_mynott 0:d33abe99b8aa 24 }
louis_mynott 0:d33abe99b8aa 25 else{
louis_mynott 0:d33abe99b8aa 26 pwmWidth *= transC*angC;
louis_mynott 0:d33abe99b8aa 27 if (pwmWidth >= 1){
louis_mynott 0:d33abe99b8aa 28 pwmWidth = 1;
louis_mynott 0:d33abe99b8aa 29 }
louis_mynott 0:d33abe99b8aa 30 if (high == 1){
louis_mynott 0:d33abe99b8aa 31 pwm.write(pwmWidth);
louis_mynott 0:d33abe99b8aa 32 }
louis_mynott 0:d33abe99b8aa 33 else{
louis_mynott 0:d33abe99b8aa 34 pwm.write(1-pwmWidth);
louis_mynott 0:d33abe99b8aa 35 }
louis_mynott 0:d33abe99b8aa 36 direction = drctn;
louis_mynott 0:d33abe99b8aa 37 }
louis_mynott 0:d33abe99b8aa 38 }
louis_mynott 0:d33abe99b8aa 39 };
louis_mynott 0:d33abe99b8aa 40
louis_mynott 0:d33abe99b8aa 41 class Encoder{
louis_mynott 0:d33abe99b8aa 42 private:
louis_mynott 0:d33abe99b8aa 43 int pulses, currentSpeed, dispx, dispy;
louis_mynott 0:d33abe99b8aa 44 Ticker tkr, disptkr;
louis_mynott 0:d33abe99b8aa 45 QEI encoder;
louis_mynott 0:d33abe99b8aa 46 public:
louis_mynott 0:d33abe99b8aa 47 Encoder(PinName CHA, PinName CHB, int x, int y):encoder(CHA, CHB, NC, 624), dispx(x), dispy(y){
louis_mynott 0:d33abe99b8aa 48 tkr.attach(callback(this, &Encoder::findPulses), 0.01);
louis_mynott 0:d33abe99b8aa 49 disptkr.attach(callback(this, &Encoder::display), 0.01);
louis_mynott 0:d33abe99b8aa 50 }
louis_mynott 0:d33abe99b8aa 51 void findPulses(){
louis_mynott 0:d33abe99b8aa 52 pulses = encoder.getPulses();
louis_mynott 0:d33abe99b8aa 53 encoder.reset(); //set back to 0 wait 0.1 seconds and take another reading
louis_mynott 0:d33abe99b8aa 54 currentSpeed = pulses*2; //set this number to circumpherance of the wheel
louis_mynott 0:d33abe99b8aa 55 tkr.attach(callback(this, &Encoder::findPulses), 0.1); //frequency of encoder readings. The longer it waits, the more accuate it is ut i nees to maintain a real time response
louis_mynott 0:d33abe99b8aa 56 }
louis_mynott 0:d33abe99b8aa 57 double findSpeed(){
louis_mynott 0:d33abe99b8aa 58 return currentSpeed;
louis_mynott 0:d33abe99b8aa 59 }
louis_mynott 0:d33abe99b8aa 60 void display(){
louis_mynott 0:d33abe99b8aa 61 lcd.locate(dispx ,dispy);
louis_mynott 0:d33abe99b8aa 62 lcd.printf("%d ", currentSpeed);
louis_mynott 0:d33abe99b8aa 63 disptkr.attach(callback(this, &Encoder::display), 0.1);
louis_mynott 0:d33abe99b8aa 64 }
louis_mynott 0:d33abe99b8aa 65 };
louis_mynott 0:d33abe99b8aa 66
louis_mynott 0:d33abe99b8aa 67
louis_mynott 0:d33abe99b8aa 68
louis_mynott 0:d33abe99b8aa 69
louis_mynott 0:d33abe99b8aa 70
louis_mynott 0:d33abe99b8aa 71
louis_mynott 0:d33abe99b8aa 72
louis_mynott 0:d33abe99b8aa 73
louis_mynott 0:d33abe99b8aa 74 //define pins
louis_mynott 0:d33abe99b8aa 75 Encoder lEncoder(PB_13, PB_14, 10, 10); //chA, chB
louis_mynott 0:d33abe99b8aa 76 Encoder rEncoder(PB_15, PB_1, 60, 10);
louis_mynott 0:d33abe99b8aa 77 Motor rMotor(PA_15, PC_13, PC_14, 0, 1); //pwm, direction, pole, ofset, logic sensitivity
louis_mynott 0:d33abe99b8aa 78 Motor lMotor(PB_7, PC_15, PH_0, 5, 0); //#######################################
louis_mynott 0:d33abe99b8aa 79 DigitalOut enable(PA_14);
louis_mynott 0:d33abe99b8aa 80
louis_mynott 0:d33abe99b8aa 81 //define global variables
louis_mynott 0:d33abe99b8aa 82
louis_mynott 0:d33abe99b8aa 83
louis_mynott 0:d33abe99b8aa 84
louis_mynott 0:d33abe99b8aa 85
louis_mynott 0:d33abe99b8aa 86
louis_mynott 0:d33abe99b8aa 87
louis_mynott 0:d33abe99b8aa 88
louis_mynott 0:d33abe99b8aa 89
louis_mynott 0:d33abe99b8aa 90 int main() {
louis_mynott 0:d33abe99b8aa 91
louis_mynott 0:d33abe99b8aa 92 double driveTime, turnTime;
louis_mynott 0:d33abe99b8aa 93 driveTime = 1; //#######################################
louis_mynott 0:d33abe99b8aa 94 turnTime = 0.8; //#######################################
louis_mynott 0:d33abe99b8aa 95 enable = 1;
louis_mynott 0:d33abe99b8aa 96
louis_mynott 0:d33abe99b8aa 97 for(int i = 0; i < 3; i++){
louis_mynott 0:d33abe99b8aa 98 lMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 99 rMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 100 wait(driveTime);
louis_mynott 0:d33abe99b8aa 101
louis_mynott 0:d33abe99b8aa 102 lMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 103 rMotor.PWM(1, 0, 1);
louis_mynott 0:d33abe99b8aa 104 wait(turnTime);
louis_mynott 0:d33abe99b8aa 105 }
louis_mynott 0:d33abe99b8aa 106
louis_mynott 0:d33abe99b8aa 107 lMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 108 rMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 109 wait(driveTime);
louis_mynott 0:d33abe99b8aa 110
louis_mynott 0:d33abe99b8aa 111 lMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 112 rMotor.PWM(1, 0, 1);
louis_mynott 0:d33abe99b8aa 113 wait(2*turnTime);
louis_mynott 0:d33abe99b8aa 114
louis_mynott 0:d33abe99b8aa 115 for(int i = 0; i < 3; i++){
louis_mynott 0:d33abe99b8aa 116 lMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 117 rMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 118 wait(driveTime);
louis_mynott 0:d33abe99b8aa 119
louis_mynott 0:d33abe99b8aa 120 lMotor.PWM(1, 0, 1);
louis_mynott 0:d33abe99b8aa 121 rMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 122 wait(turnTime);
louis_mynott 0:d33abe99b8aa 123 }
louis_mynott 0:d33abe99b8aa 124
louis_mynott 0:d33abe99b8aa 125 lMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 126 rMotor.PWM(1, 1, 1);
louis_mynott 0:d33abe99b8aa 127 wait(driveTime);
louis_mynott 0:d33abe99b8aa 128
louis_mynott 0:d33abe99b8aa 129 lMotor.PWM(0, 1, 1);
louis_mynott 0:d33abe99b8aa 130 rMotor.PWM(0, 1, 1);
louis_mynott 0:d33abe99b8aa 131 }