2015-2016_Mouserat / Lab1_Test

hello

Dependencies:   AVEncoder QEI mbed-src-AV

main.cpp@2:5f9b78950a17, 2015-11-10 (annotated)

Committer:
intgsull
Date:
Tue Nov 10 07:29:32 2015 +0000
Revision:
2:5f9b78950a17
Parent:
1:5b9fa1823663
Child:
4:453d534b1c1d
Lab 2 Complete!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
intgsull 0:fa523db3f4f5 1 #include "mbed.h"
intgsull 1:5b9fa1823663 2 #include "QEI.h"
intgsull 0:fa523db3f4f5 3
intgsull 2:5f9b78950a17 4 PwmOut motor1_reverse(PA_6);
intgsull 1:5b9fa1823663 5 PwmOut motor1_forward(PB_10);
intgsull 0:fa523db3f4f5 6 PwmOut motor2_forward(PA_7);
intgsull 0:fa523db3f4f5 7 PwmOut motor2_reverse(PB_6);
intgsull 0:fa523db3f4f5 8
intgsull 2:5f9b78950a17 9 //DigitalIn encoder1A(PA_15);
intgsull 2:5f9b78950a17 10 //DigitalIn encoder1B(PB_3);
intgsull 2:5f9b78950a17 11 //DigitalIn encoder2A(PA_1);
intgsull 2:5f9b78950a17 12 //DigitalIn encoder2B(PA_10);
intgsull 2:5f9b78950a17 13
intgsull 2:5f9b78950a17 14
intgsull 2:5f9b78950a17 15
intgsull 2:5f9b78950a17 16 //Gyro
intgsull 2:5f9b78950a17 17 AnalogIn _gyro(PA_0);
intgsull 2:5f9b78950a17 18 AnalogIn gyro_cal(PC_1); //no
intgsull 1:5b9fa1823663 19
intgsull 2:5f9b78950a17 20 //Left Front IR
intgsull 2:5f9b78950a17 21 DigitalOut eLF(PC_3);
intgsull 2:5f9b78950a17 22 AnalogIn rLF(PC_0);
intgsull 2:5f9b78950a17 23 //PC_4 is an ADC
intgsull 2:5f9b78950a17 24
intgsull 2:5f9b78950a17 25 //Left Side IR
intgsull 2:5f9b78950a17 26 DigitalOut eLS(PC_2);
intgsull 2:5f9b78950a17 27 AnalogIn rLS(PC_1);
intgsull 2:5f9b78950a17 28
intgsull 2:5f9b78950a17 29 //Right Front IR
intgsull 2:5f9b78950a17 30 DigitalOut eRF(PC_12);
intgsull 2:5f9b78950a17 31 AnalogIn rRF(PA_4);
intgsull 2:5f9b78950a17 32
intgsull 2:5f9b78950a17 33 //Right Side IR
intgsull 2:5f9b78950a17 34 DigitalOut eRS(PC_15);
intgsull 2:5f9b78950a17 35 AnalogIn rRS(PB_0);
intgsull 1:5b9fa1823663 36
intgsull 0:fa523db3f4f5 37 DigitalOut myled(LED1);
intgsull 0:fa523db3f4f5 38
intgsull 2:5f9b78950a17 39 QEI LENC (PA_15, PB_3, NC, 100 );
intgsull 1:5b9fa1823663 40 QEI RENC (PA_1, PA_10, NC, 100 );
intgsull 1:5b9fa1823663 41
intgsull 1:5b9fa1823663 42 int distance = 0;
intgsull 1:5b9fa1823663 43
intgsull 0:fa523db3f4f5 44 void rightForward(float speed);
intgsull 0:fa523db3f4f5 45 void leftForward(float speed);
intgsull 0:fa523db3f4f5 46
intgsull 2:5f9b78950a17 47 float read_gyro( void );
intgsull 2:5f9b78950a17 48
intgsull 2:5f9b78950a17 49 Serial pc (SERIAL_TX, SERIAL_RX);
intgsull 1:5b9fa1823663 50
intgsull 0:fa523db3f4f5 51 int main() {
intgsull 2:5f9b78950a17 52 // LENC.reset();
intgsull 2:5f9b78950a17 53 // RENC.reset();
intgsull 2:5f9b78950a17 54 eRS = 0;
intgsull 2:5f9b78950a17 55 eRF = 0;
intgsull 2:5f9b78950a17 56 eLS = 0;
intgsull 2:5f9b78950a17 57 eLF = 0;
intgsull 2:5f9b78950a17 58
intgsull 2:5f9b78950a17 59 myled = 1;
intgsull 1:5b9fa1823663 60 while(1) {
intgsull 2:5f9b78950a17 61
intgsull 1:5b9fa1823663 62 motor1_reverse = 0;
intgsull 1:5b9fa1823663 63 motor1_forward = 0;
intgsull 0:fa523db3f4f5 64
intgsull 1:5b9fa1823663 65 motor2_forward = 0;
intgsull 1:5b9fa1823663 66 motor2_reverse = 0;
intgsull 1:5b9fa1823663 67
intgsull 2:5f9b78950a17 68 eRS = 0;
intgsull 2:5f9b78950a17 69 eRF = 0;
intgsull 2:5f9b78950a17 70 eLS = 0;
intgsull 2:5f9b78950a17 71 eLF = 0;
intgsull 2:5f9b78950a17 72
intgsull 2:5f9b78950a17 73 pc.printf("LEDS off\r\n");
intgsull 2:5f9b78950a17 74 wait(1);
intgsull 2:5f9b78950a17 75 pc.printf("Left Front: \t%f\r\n", rLF.read());
intgsull 2:5f9b78950a17 76 pc.printf("Left Side: \t%f\r\n", rLS.read());
intgsull 2:5f9b78950a17 77 pc.printf("Right Front: \t%f\r\n", rRS.read());
intgsull 2:5f9b78950a17 78 pc.printf("Right Side: \t%f\r\n\n", rRF.read());
intgsull 2:5f9b78950a17 79 wait(.5);
intgsull 2:5f9b78950a17 80
intgsull 2:5f9b78950a17 81 eRS = 1;
intgsull 2:5f9b78950a17 82 eRF = 1;
intgsull 2:5f9b78950a17 83 eLS = 1;
intgsull 2:5f9b78950a17 84 eLF = 1;
intgsull 2:5f9b78950a17 85
intgsull 2:5f9b78950a17 86 pc.printf("LEDS on\r\n");
intgsull 2:5f9b78950a17 87 wait(1);
intgsull 2:5f9b78950a17 88 pc.printf("Left Front: \t%f\r\n", rLF.read());
intgsull 2:5f9b78950a17 89 pc.printf("Left Side: \t%f\r\n", rLS.read());
intgsull 2:5f9b78950a17 90 pc.printf("Right Front: \t%f\r\n", rRS.read());
intgsull 2:5f9b78950a17 91 pc.printf("Right Side: \t%f\r\n\n", rRF.read());
intgsull 2:5f9b78950a17 92 // pc.printf("%f\r\n", _gyro.read());
intgsull 2:5f9b78950a17 93 // wait(0.5);
intgsull 2:5f9b78950a17 94 }
intgsull 1:5b9fa1823663 95
intgsull 2:5f9b78950a17 96
intgsull 2:5f9b78950a17 97 // int in = encoder1A.read();
intgsull 2:5f9b78950a17 98 // if(in==1)
intgsull 2:5f9b78950a17 99 // myled=1;
intgsull 2:5f9b78950a17 100 // else
intgsull 2:5f9b78950a17 101 // myled=0;
intgsull 2:5f9b78950a17 102
intgsull 2:5f9b78950a17 103 //}
intgsull 1:5b9fa1823663 104 //
intgsull 1:5b9fa1823663 105 // motor1_reverse = 1.0;
intgsull 1:5b9fa1823663 106 // motor1_forward = 1.0;
intgsull 1:5b9fa1823663 107 //
intgsull 1:5b9fa1823663 108 // motor2_forward = 1.0;
intgsull 1:5b9fa1823663 109 // motor2_reverse = 1.0;
intgsull 1:5b9fa1823663 110 //rightForward(0.5);
intgsull 1:5b9fa1823663 111 //leftForward(-0.5);
intgsull 0:fa523db3f4f5 112 }
intgsull 2:5f9b78950a17 113
intgsull 2:5f9b78950a17 114
intgsull 2:5f9b78950a17 115 float read_gyro ( void ){
intgsull 2:5f9b78950a17 116 // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset
intgsull 2:5f9b78950a17 117 float r_gyro = _gyro.read(); // Get the Analog value from the STM32
intgsull 2:5f9b78950a17 118 r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
intgsull 2:5f9b78950a17 119 r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign
intgsull 2:5f9b78950a17 120 r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise)
intgsull 2:5f9b78950a17 121 return r_gyro;
intgsull 2:5f9b78950a17 122 }
intgsull 2:5f9b78950a17 123
intgsull 1:5b9fa1823663 124 void moveForward(float speed)
intgsull 1:5b9fa1823663 125 {
intgsull 1:5b9fa1823663 126 motor1_forward = speed;
intgsull 1:5b9fa1823663 127 motor2_forward = speed;
intgsull 1:5b9fa1823663 128
intgsull 1:5b9fa1823663 129 motor1_reverse = 0;
intgsull 1:5b9fa1823663 130 motor2_reverse = 0;
intgsull 1:5b9fa1823663 131 }
intgsull 1:5b9fa1823663 132
intgsull 1:5b9fa1823663 133
intgsull 1:5b9fa1823663 134
intgsull 0:fa523db3f4f5 135 void rightForward(float speed) {
intgsull 0:fa523db3f4f5 136 if (speed == 0) {
intgsull 0:fa523db3f4f5 137 motor1_forward = 1.0;
intgsull 0:fa523db3f4f5 138 motor1_reverse = 1.0;
intgsull 0:fa523db3f4f5 139 }
intgsull 0:fa523db3f4f5 140
intgsull 0:fa523db3f4f5 141 if (speed > 0) {
intgsull 0:fa523db3f4f5 142 motor1_forward = speed;
intgsull 0:fa523db3f4f5 143 motor1_reverse = 0;
intgsull 0:fa523db3f4f5 144 }
intgsull 0:fa523db3f4f5 145
intgsull 0:fa523db3f4f5 146 else {
intgsull 0:fa523db3f4f5 147 motor1_forward = 0;
intgsull 0:fa523db3f4f5 148 motor1_reverse = -speed;
intgsull 0:fa523db3f4f5 149 }
intgsull 0:fa523db3f4f5 150 }
intgsull 0:fa523db3f4f5 151
intgsull 0:fa523db3f4f5 152 void leftForward(float speed) {
intgsull 0:fa523db3f4f5 153 if (speed == 0) {
intgsull 0:fa523db3f4f5 154 motor2_forward = 1.0;
intgsull 0:fa523db3f4f5 155 motor2_reverse = 1.0;
intgsull 0:fa523db3f4f5 156 }
intgsull 0:fa523db3f4f5 157
intgsull 0:fa523db3f4f5 158 if (speed > 0) {
intgsull 0:fa523db3f4f5 159 motor2_forward = speed;
intgsull 0:fa523db3f4f5 160 motor2_reverse = 0;
intgsull 0:fa523db3f4f5 161 }
intgsull 0:fa523db3f4f5 162
intgsull 0:fa523db3f4f5 163 else {
intgsull 0:fa523db3f4f5 164 motor2_forward = 0;
intgsull 0:fa523db3f4f5 165 motor2_reverse = -speed;
intgsull 0:fa523db3f4f5 166 }
intgsull 0:fa523db3f4f5 167 }