hisyam fs / Mbed 2 deprecated Test_all

tes ir atas semua

Dependencies:   mbed ADS1115 StepperMotor SRF05 TPA81new

main.cpp@7:a6dc7ec6e4c0, 2018-02-09 (annotated)

Committer:
Ezeuz
Date:
Fri Feb 09 17:23:27 2018 +0000
Revision:
7:a6dc7ec6e4c0
Parent:
6:69c59bcab6ea
Child:
8:5e1854c119ba
Update

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 5:dae415fb4bad 30 // Settings // Servo comm : tx, rx, txEn, id, baud
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 6:69c59bcab6ea 42 AnalogIn line2(PC_4); // 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 6:69c59bcab6ea 45 CMPS11 cmp(PB_4, PA_8, 0xC0); // Compass : I2C3_SDA, I2C3_SCL
Ezeuz 4:9932af380e56 46
Ezeuz 4:9932af380e56 47 /* About interrupt
Ezeuz 4:9932af380e56 48 Where are the interrupt pins on NUCLEO-F411RE?
Ezeuz 4:9932af380e56 49
Ezeuz 4:9932af380e56 50 If you use a recent version of the mbed lib (right mouse button, update in the online compiler):
Ezeuz 4:9932af380e56 51 Every unique numbered pin. That means you can use any pin as InterruptIn, but you cannot use multiple
Ezeuz 4:9932af380e56 52 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 53 But in this example you could not use also PE_1. */
dmgongora 0:79e2a8171b16 54
dmgongora 0:79e2a8171b16 55 int main()
Ezeuz 4:9932af380e56 56 {
Ezeuz 5:dae415fb4bad 57 // Servos
Ezeuz 5:dae415fb4bad 58 for (int i = 1; i <= 18; i++) {
Ezeuz 5:dae415fb4bad 59 Dynamixel servo(PC_6, PC_7, PC_4, i, 1000000);
Ezeuz 5:dae415fb4bad 60 servo.setSpeed(100);
Ezeuz 5:dae415fb4bad 61 servo.move(512); // Midddle, thus 90 deg position
Ezeuz 5:dae415fb4bad 62 }
Ezeuz 5:dae415fb4bad 63
Ezeuz 6:69c59bcab6ea 64 float line1_o = 0;
Ezeuz 6:69c59bcab6ea 65 float line2_o = 0;
Ezeuz 4:9932af380e56 66 float meas = 0;
Ezeuz 7:a6dc7ec6e4c0 67 float snd = 0;
Ezeuz 7:a6dc7ec6e4c0 68 float uvo = 0;
Ezeuz 6:69c59bcab6ea 69 int ext = 0;
Ezeuz 6:69c59bcab6ea 70
Ezeuz 6:69c59bcab6ea 71 // Lidar
Ezeuz 6:69c59bcab6ea 72 lidar.StartData();
Ezeuz 6:69c59bcab6ea 73 int i = 0;
Ezeuz 6:69c59bcab6ea 74 float data, speed;
Ezeuz 6:69c59bcab6ea 75 short intensity;
Ezeuz 6:69c59bcab6ea 76 bool invalid_flag, strength_flag;
Ezeuz 6:69c59bcab6ea 77
Ezeuz 6:69c59bcab6ea 78 // Compass
Ezeuz 6:69c59bcab6ea 79 int val;
Ezeuz 6:69c59bcab6ea 80 int16_t mx,my,mz;
Ezeuz 6:69c59bcab6ea 81 cmp.reset();
Ezeuz 6:69c59bcab6ea 82 /*
Ezeuz 6:69c59bcab6ea 83 cmp.startCalibrate(true);
Ezeuz 6:69c59bcab6ea 84 while(button){ };
Ezeuz 6:69c59bcab6ea 85 pc.printf("Calibrate done\n");
Ezeuz 6:69c59bcab6ea 86 wait_ms(500);
Ezeuz 6:69c59bcab6ea 87 cmp.stopCalibrate();
Ezeuz 6:69c59bcab6ea 88 */
dmgongora 0:79e2a8171b16 89
Ezeuz 4:9932af380e56 90 while (1) {
Ezeuz 4:9932af380e56 91 // LCD
Ezeuz 7:a6dc7ec6e4c0 92 lcd.printf("%.1f L%.2f|%.2f\n", meas, line1_o, line2_o);
Ezeuz 7:a6dc7ec6e4c0 93 pc.printf("%.2fcm L%.2f|%.2f ", meas, line1_o, line2_o);
Ezeuz 5:dae415fb4bad 94
Ezeuz 7:a6dc7ec6e4c0 95 lcd.printf("s%d u%d\n", (int)snd, (int)uvo);
Ezeuz 6:69c59bcab6ea 96 pc.printf("s%d u%d ", snd, uvo);
Ezeuz 4:9932af380e56 97
Ezeuz 4:9932af380e56 98 // IR
Ezeuz 4:9932af380e56 99 meas = ir.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
Ezeuz 5:dae415fb4bad 100 meas = ((1-meas)*26+4);
Ezeuz 4:9932af380e56 101
Ezeuz 6:69c59bcab6ea 102 // Line Sensor
Ezeuz 6:69c59bcab6ea 103 line1_o = line1.read();
Ezeuz 6:69c59bcab6ea 104 line2_o = line2.read();
Ezeuz 6:69c59bcab6ea 105
Ezeuz 4:9932af380e56 106 // Extinguisher or 12V output
Ezeuz 6:69c59bcab6ea 107 m2 = ext;
Ezeuz 6:69c59bcab6ea 108 m1 = ext;
Ezeuz 6:69c59bcab6ea 109 ext = !ext;
Ezeuz 4:9932af380e56 110
Ezeuz 4:9932af380e56 111 // LED
Ezeuz 4:9932af380e56 112 led1 = 1;
Ezeuz 4:9932af380e56 113 led2 = 1;
Ezeuz 4:9932af380e56 114
Ezeuz 4:9932af380e56 115 // Sound Activator
Ezeuz 4:9932af380e56 116 snd = sound.read();
Ezeuz 5:dae415fb4bad 117
Ezeuz 5:dae415fb4bad 118 // UV
Ezeuz 5:dae415fb4bad 119 uvo = uv.read();
Ezeuz 6:69c59bcab6ea 120
Ezeuz 6:69c59bcab6ea 121 // Lidar
Ezeuz 6:69c59bcab6ea 122 // Aquire LIDAR data from angle 0, 45, 90, 135... 315
Ezeuz 6:69c59bcab6ea 123 // Then send it to serial PC with 9600 baud
Ezeuz 6:69c59bcab6ea 124 data = lidar.GetData(i);
Ezeuz 6:69c59bcab6ea 125 speed = lidar.GetSpeed();
Ezeuz 6:69c59bcab6ea 126 intensity = lidar.GetIntensity(i);
Ezeuz 6:69c59bcab6ea 127 invalid_flag = lidar.IsDataInvalid(i);
Ezeuz 6:69c59bcab6ea 128 strength_flag = lidar.GetStrengthFlag(i);
Ezeuz 6:69c59bcab6ea 129
Ezeuz 6:69c59bcab6ea 130 pc.printf("Spd=%.1f; Sdt=%d; D=%.1f; I=%.1d; finvalid=%d; fstrength=%d\t", speed, i, data, intensity, invalid_flag, strength_flag);
Ezeuz 6:69c59bcab6ea 131
Ezeuz 6:69c59bcab6ea 132 /*
Ezeuz 6:69c59bcab6ea 133 i += 90;
Ezeuz 6:69c59bcab6ea 134 if (i >= 360) {
Ezeuz 6:69c59bcab6ea 135 i = 0;
Ezeuz 6:69c59bcab6ea 136 }
Ezeuz 6:69c59bcab6ea 137 */
Ezeuz 6:69c59bcab6ea 138
Ezeuz 6:69c59bcab6ea 139 // Compass
Ezeuz 6:69c59bcab6ea 140 val = cmp.readBearing();
Ezeuz 6:69c59bcab6ea 141 mx = cmp.mag_x();
Ezeuz 6:69c59bcab6ea 142 my = cmp.mag_y();
Ezeuz 6:69c59bcab6ea 143 mz = cmp.mag_z();
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 6:69c59bcab6ea 146 /* if(!button){
Ezeuz 6:69c59bcab6ea 147 pc.printf("Reset/n");
Ezeuz 6:69c59bcab6ea 148 cmp.reset();
Ezeuz 6:69c59bcab6ea 149 } */
Ezeuz 6:69c59bcab6ea 150
Ezeuz 6:69c59bcab6ea 151 pc.printf("\n");
Ezeuz 6:69c59bcab6ea 152
Ezeuz 6:69c59bcab6ea 153 wait(0.2);
Ezeuz 4:9932af380e56 154 }
dmgongora 0:79e2a8171b16 155 }