hisyam fs / Mbed 2 deprecated Test_all

tes ir atas semua

Dependencies:   mbed ADS1115 StepperMotor SRF05 TPA81new

main.cpp@8:5e1854c119ba, 2018-02-13 (annotated)

Committer:
Ezeuz
Date:
Tue Feb 13 16:29:00 2018 +0000
Revision:
8:5e1854c119ba
Parent:
7:a6dc7ec6e4c0
Child:
9:ba07c0b8899f
Compass, serial input management, button management, etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dmgongora 1:ef90d942ce78 1 /*****************************************************
dmgongora 1:ef90d942ce78 2 - Description: mbed to Dynamixel connection test using
dmgongora 1:ef90d942ce78 3 the library
dmgongora 1:ef90d942ce78 4 - Requirements:
dmgongora 1:ef90d942ce78 5 Dynamixel (i.e. DX116, RX28)
dmgongora 1:ef90d942ce78 6 MAX3088/MAX485 (RS485 transceiver)
dmgongora 1:ef90d942ce78 7 - Connections:
dmgongora 1:ef90d942ce78 8 MAX3088 -- mbed
dmgongora 1:ef90d942ce78 9 ======================
dmgongora 1:ef90d942ce78 10 Pin 1 -- Pin 14
dmgongora 1:ef90d942ce78 11 Pin 2 -- Pin 15
dmgongora 1:ef90d942ce78 12 Pin 4 -- Pin 13
dmgongora 1:ef90d942ce78 13
dmgongora 1:ef90d942ce78 14 - Comments:
dmgongora 1:ef90d942ce78 15 See schematic for wiring details and class
dmgongora 1:ef90d942ce78 16 documentation for available methods.
dmgongora 1:ef90d942ce78 17 *****************************************************/
Ezeuz 4:9932af380e56 18
dmgongora 0:79e2a8171b16 19 #include "mbed.h"
dmgongora 0:79e2a8171b16 20 #include "Dynamixel.h"
Ezeuz 4:9932af380e56 21 #include "TextLCD.h"
Ezeuz 4:9932af380e56 22 #include "Uvtron.h"
Ezeuz 4:9932af380e56 23
Ezeuz 6:69c59bcab6ea 24 #include "CMPS11.h"
Ezeuz 6:69c59bcab6ea 25 #include "LIDAR.h"
Ezeuz 6:69c59bcab6ea 26
Ezeuz 4:9932af380e56 27 // Defines
Ezeuz 4:9932af380e56 28 #define IR_CONST 1.229
Ezeuz 4:9932af380e56 29
Ezeuz 8:5e1854c119ba 30 // Settings
Ezeuz 5:dae415fb4bad 31 TextLCD lcd(PA_5, PA_11, PA_6, PB_12, PA_7, PB_6); // LCD : rs, e, d4-d7
Ezeuz 5:dae415fb4bad 32 AnalogIn ir(PC_5); // Sharp IR : analog
Ezeuz 7:a6dc7ec6e4c0 33 DigitalOut m2(PA_15); // extinguisher : 12V out M2, possibly broken
Ezeuz 7:a6dc7ec6e4c0 34 DigitalOut m1(PB_7); // extinguisher : 12V out M1
Ezeuz 4:9932af380e56 35 DigitalOut led1(PC_13); // GPIO high is 3V3
Ezeuz 4:9932af380e56 36 DigitalOut led2(PC_14);
Ezeuz 6:69c59bcab6ea 37 DigitalIn sound(PA_12); // Sound act : digital, active low
Ezeuz 5:dae415fb4bad 38 DigitalIn uv(PB_8); // UVTron : digital, pin 2 = active low
Ezeuz 6:69c59bcab6ea 39 Serial pc(USBTX, USBRX); // tx, rx
Ezeuz 4:9932af380e56 40
Ezeuz 6:69c59bcab6ea 41 AnalogIn line1(PB_1); // Line sensor : analog (or digital)
Ezeuz 8:5e1854c119ba 42 AnalogIn line2(PA_0); // Line sensor : analog (or digital)
Ezeuz 6:69c59bcab6ea 43
Ezeuz 6:69c59bcab6ea 44 LIDAR lidar (PC_10, PC_11, PA_1); // Lidar : tx,rx, motor PWM
Ezeuz 8:5e1854c119ba 45 CMPS11 cmp(PB_4, PA_8, 0xC0); // Compass : I2C3_SDA, I2C3_SCL, Addr
Ezeuz 8:5e1854c119ba 46
Ezeuz 8:5e1854c119ba 47 DigitalIn button(USER_BUTTON); // Button
Ezeuz 4:9932af380e56 48
Ezeuz 4:9932af380e56 49 /* About interrupt
Ezeuz 4:9932af380e56 50 Where are the interrupt pins on NUCLEO-F411RE?
Ezeuz 4:9932af380e56 51
Ezeuz 8:5e1854c119ba 52 If you're using a recent version of the mbed lib (right mouse button, update in the online compiler):
Ezeuz 4:9932af380e56 53 Every unique numbered pin. That means you can use any pin as InterruptIn, but you cannot use multiple
Ezeuz 4:9932af380e56 54 pins with the same number on a different port as InterruptIn. So you can use PA_1, PB_2, PA_3, PC_4, etc.
Ezeuz 4:9932af380e56 55 But in this example you could not use also PE_1. */
dmgongora 0:79e2a8171b16 56
dmgongora 0:79e2a8171b16 57 int main()
Ezeuz 4:9932af380e56 58 {
Ezeuz 5:dae415fb4bad 59 // Servos
Ezeuz 5:dae415fb4bad 60 for (int i = 1; i <= 18; i++) {
Ezeuz 8:5e1854c119ba 61 Dynamixel servo(PC_6, PC_7, PC_4, i, 1000000); // Servo comm : tx, rx, txEn, id, baud
Ezeuz 5:dae415fb4bad 62 servo.setSpeed(100);
Ezeuz 5:dae415fb4bad 63 servo.move(512); // Midddle, thus 90 deg position
Ezeuz 5:dae415fb4bad 64 }
Ezeuz 5:dae415fb4bad 65
Ezeuz 6:69c59bcab6ea 66 float line1_o = 0;
Ezeuz 6:69c59bcab6ea 67 float line2_o = 0;
Ezeuz 8:5e1854c119ba 68 float ir_o = 0;
Ezeuz 8:5e1854c119ba 69 float snd_o = 0;
Ezeuz 8:5e1854c119ba 70 float uv_o = 0;
Ezeuz 8:5e1854c119ba 71 int ext = 0; // Extinguisher state
Ezeuz 8:5e1854c119ba 72
Ezeuz 8:5e1854c119ba 73 int tcal = 0; // Toggle calibration
Ezeuz 8:5e1854c119ba 74 int fcal = 0; // Force calibration
Ezeuz 8:5e1854c119ba 75 int ptcal = 0; // Prev toggle calibration
Ezeuz 8:5e1854c119ba 76 int calib = 0; // Calibration state
Ezeuz 8:5e1854c119ba 77
Ezeuz 8:5e1854c119ba 78 char ser_i; // Get data from serial input buffer
Ezeuz 6:69c59bcab6ea 79
Ezeuz 6:69c59bcab6ea 80 // Lidar
Ezeuz 6:69c59bcab6ea 81 lidar.StartData();
Ezeuz 6:69c59bcab6ea 82 int i = 0;
Ezeuz 6:69c59bcab6ea 83 float data, speed;
Ezeuz 6:69c59bcab6ea 84 short intensity;
Ezeuz 6:69c59bcab6ea 85 bool invalid_flag, strength_flag;
Ezeuz 6:69c59bcab6ea 86
Ezeuz 6:69c59bcab6ea 87 // Compass
Ezeuz 6:69c59bcab6ea 88 int val;
Ezeuz 6:69c59bcab6ea 89 int16_t mx,my,mz;
Ezeuz 6:69c59bcab6ea 90 cmp.reset();
dmgongora 0:79e2a8171b16 91
Ezeuz 4:9932af380e56 92 while (1) {
Ezeuz 4:9932af380e56 93 // LCD
Ezeuz 8:5e1854c119ba 94 lcd.printf("%.1f L%.2f|%.2f\n", ir_o, line1_o, line2_o);
Ezeuz 8:5e1854c119ba 95 pc.printf("%.2fcm L%.2f|%.2f ", ir_o, line1_o, line2_o);
Ezeuz 5:dae415fb4bad 96
Ezeuz 8:5e1854c119ba 97 lcd.printf("s%d u%d", (int)snd_o, (int)uv_o);
Ezeuz 8:5e1854c119ba 98 pc.printf("s%.2f u%.2f ", snd_o, uv_o);
Ezeuz 4:9932af380e56 99
Ezeuz 4:9932af380e56 100 // IR
Ezeuz 8:5e1854c119ba 101 ir_o = ((1-ir.read())*26+4); // Convert and read the analog input value (value from 0.0 to 1.0)
Ezeuz 4:9932af380e56 102
Ezeuz 6:69c59bcab6ea 103 // Line Sensor
Ezeuz 6:69c59bcab6ea 104 line1_o = line1.read();
Ezeuz 6:69c59bcab6ea 105 line2_o = line2.read();
Ezeuz 6:69c59bcab6ea 106
Ezeuz 8:5e1854c119ba 107 // Extinguisher (12V output)
Ezeuz 6:69c59bcab6ea 108 m2 = ext;
Ezeuz 6:69c59bcab6ea 109 m1 = ext;
Ezeuz 8:5e1854c119ba 110 ext = !ext; // Switching test
Ezeuz 4:9932af380e56 111
Ezeuz 4:9932af380e56 112 // LED
Ezeuz 4:9932af380e56 113 led1 = 1;
Ezeuz 4:9932af380e56 114 led2 = 1;
Ezeuz 4:9932af380e56 115
Ezeuz 4:9932af380e56 116 // Sound Activator
Ezeuz 8:5e1854c119ba 117 snd_o = sound.read();
Ezeuz 5:dae415fb4bad 118
Ezeuz 5:dae415fb4bad 119 // UV
Ezeuz 8:5e1854c119ba 120 uv_o = uv.read();
Ezeuz 6:69c59bcab6ea 121
Ezeuz 6:69c59bcab6ea 122 // Lidar
Ezeuz 6:69c59bcab6ea 123 // Aquire LIDAR data from angle 0, 45, 90, 135... 315
Ezeuz 6:69c59bcab6ea 124 // Then send it to serial PC with 9600 baud
Ezeuz 8:5e1854c119ba 125
Ezeuz 8:5e1854c119ba 126 // i += 90;
Ezeuz 8:5e1854c119ba 127 // if (i >= 360) {
Ezeuz 8:5e1854c119ba 128 // i = 0;
Ezeuz 6:69c59bcab6ea 129 data = lidar.GetData(i);
Ezeuz 6:69c59bcab6ea 130 speed = lidar.GetSpeed();
Ezeuz 6:69c59bcab6ea 131 intensity = lidar.GetIntensity(i);
Ezeuz 6:69c59bcab6ea 132 invalid_flag = lidar.IsDataInvalid(i);
Ezeuz 6:69c59bcab6ea 133 strength_flag = lidar.GetStrengthFlag(i);
Ezeuz 6:69c59bcab6ea 134
Ezeuz 6:69c59bcab6ea 135 pc.printf("Spd=%.1f; Sdt=%d; D=%.1f; I=%.1d; finvalid=%d; fstrength=%d\t", speed, i, data, intensity, invalid_flag, strength_flag);
Ezeuz 8:5e1854c119ba 136 //}
Ezeuz 6:69c59bcab6ea 137
Ezeuz 6:69c59bcab6ea 138 // Compass
Ezeuz 6:69c59bcab6ea 139 val = cmp.readBearing();
Ezeuz 6:69c59bcab6ea 140 mx = cmp.mag_x();
Ezeuz 6:69c59bcab6ea 141 my = cmp.mag_y();
Ezeuz 6:69c59bcab6ea 142 mz = cmp.mag_z();
Ezeuz 8:5e1854c119ba 143 lcd.printf(" %.1f", (float)val/10);
Ezeuz 6:69c59bcab6ea 144 pc.printf("%d.%d\t", val/10, val%10);
Ezeuz 6:69c59bcab6ea 145 pc.printf("%d %d %d\t", mx, my, mz);
Ezeuz 8:5e1854c119ba 146
Ezeuz 8:5e1854c119ba 147 // Standard serial management
Ezeuz 8:5e1854c119ba 148 while(pc.readable()) {
Ezeuz 8:5e1854c119ba 149 // Read serial buffer until it's empty
Ezeuz 8:5e1854c119ba 150 ser_i = pc.getc();
Ezeuz 8:5e1854c119ba 151 switch(ser_i) {
Ezeuz 8:5e1854c119ba 152 case 'r':
Ezeuz 8:5e1854c119ba 153 pc.printf("Compass Reset\n");
Ezeuz 8:5e1854c119ba 154 cmp.reset();
Ezeuz 8:5e1854c119ba 155 break;
Ezeuz 8:5e1854c119ba 156 case 'c':
Ezeuz 8:5e1854c119ba 157 pc.printf("Compass Calib\n");
Ezeuz 8:5e1854c119ba 158 fcal = !fcal;
Ezeuz 8:5e1854c119ba 159 break;
Ezeuz 8:5e1854c119ba 160 }
Ezeuz 8:5e1854c119ba 161 // pc.putc(ser_i); // Display serial input
Ezeuz 8:5e1854c119ba 162 }
Ezeuz 6:69c59bcab6ea 163
Ezeuz 8:5e1854c119ba 164 // Standard button management
Ezeuz 8:5e1854c119ba 165 //tcal = !button.read();
Ezeuz 8:5e1854c119ba 166 //tcal = !uv_o; // Using UVTron to avoid using button
Ezeuz 8:5e1854c119ba 167 tcal = snd_o;
Ezeuz 8:5e1854c119ba 168
Ezeuz 8:5e1854c119ba 169 // cmp.reset();
Ezeuz 8:5e1854c119ba 170 if((!tcal && (tcal != ptcal)) || fcal) { // Rising or forced
Ezeuz 8:5e1854c119ba 171 if(!calib) {
Ezeuz 8:5e1854c119ba 172 cmp.startCalibrate(1);
Ezeuz 8:5e1854c119ba 173 calib = 1;
Ezeuz 8:5e1854c119ba 174 } else cmp.stopCalibrate();
Ezeuz 8:5e1854c119ba 175 fcal = 0;
Ezeuz 8:5e1854c119ba 176 }
Ezeuz 8:5e1854c119ba 177 ptcal = tcal;
Ezeuz 8:5e1854c119ba 178
Ezeuz 8:5e1854c119ba 179 pc.printf(" B%d|%d", tcal, ptcal);
Ezeuz 8:5e1854c119ba 180
Ezeuz 8:5e1854c119ba 181 lcd.printf("\n");
Ezeuz 6:69c59bcab6ea 182 pc.printf("\n");
Ezeuz 6:69c59bcab6ea 183
Ezeuz 6:69c59bcab6ea 184 wait(0.2);
Ezeuz 4:9932af380e56 185 }
dmgongora 0:79e2a8171b16 186 }