hisyam fs / Mbed 2 deprecated Test_all

tes ir atas semua

Dependencies:   mbed ADS1115 StepperMotor SRF05 TPA81new

main.cpp@10:8722053fb75c, 2018-02-16 (annotated)

Committer:
Ezeuz
Date:
Fri Feb 16 12:43:38 2018 +0000
Revision:
10:8722053fb75c
Parent:
9:ba07c0b8899f
Child:
12:1e3227a6fcd7
-

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 10:8722053fb75c 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 9:ba07c0b8899f 92 // Motor switching test
Ezeuz 9:ba07c0b8899f 93 m2 = 0;
Ezeuz 9:ba07c0b8899f 94 m1 = 0;
Ezeuz 9:ba07c0b8899f 95
Ezeuz 9:ba07c0b8899f 96 wait(1);
Ezeuz 9:ba07c0b8899f 97
Ezeuz 4:9932af380e56 98 while (1) {
Ezeuz 4:9932af380e56 99 // LCD
Ezeuz 8:5e1854c119ba 100 lcd.printf("%.1f L%.2f|%.2f\n", ir_o, line1_o, line2_o);
Ezeuz 10:8722053fb75c 101 pc.printf("%.2fcm L%.2f|%.2f M%d ", ir_o, line1_o, line2_o, ext);
Ezeuz 5:dae415fb4bad 102
Ezeuz 8:5e1854c119ba 103 lcd.printf("s%d u%d", (int)snd_o, (int)uv_o);
Ezeuz 8:5e1854c119ba 104 pc.printf("s%.2f u%.2f ", snd_o, uv_o);
Ezeuz 4:9932af380e56 105
Ezeuz 4:9932af380e56 106 // IR
Ezeuz 8:5e1854c119ba 107 ir_o = ((1-ir.read())*26+4); // Convert and read the analog input value (value from 0.0 to 1.0)
Ezeuz 4:9932af380e56 108
Ezeuz 6:69c59bcab6ea 109 // Line Sensor
Ezeuz 6:69c59bcab6ea 110 line1_o = line1.read();
Ezeuz 6:69c59bcab6ea 111 line2_o = line2.read();
Ezeuz 6:69c59bcab6ea 112
Ezeuz 8:5e1854c119ba 113 // Extinguisher (12V output)
Ezeuz 6:69c59bcab6ea 114 m2 = ext;
Ezeuz 6:69c59bcab6ea 115 m1 = ext;
Ezeuz 4:9932af380e56 116
Ezeuz 4:9932af380e56 117 // LED
Ezeuz 4:9932af380e56 118 led1 = 1;
Ezeuz 4:9932af380e56 119 led2 = 1;
Ezeuz 4:9932af380e56 120
Ezeuz 4:9932af380e56 121 // Sound Activator
Ezeuz 8:5e1854c119ba 122 snd_o = sound.read();
Ezeuz 5:dae415fb4bad 123
Ezeuz 5:dae415fb4bad 124 // UV
Ezeuz 8:5e1854c119ba 125 uv_o = uv.read();
Ezeuz 6:69c59bcab6ea 126
Ezeuz 6:69c59bcab6ea 127 // Lidar
Ezeuz 6:69c59bcab6ea 128 // Aquire LIDAR data from angle 0, 45, 90, 135... 315
Ezeuz 6:69c59bcab6ea 129 // Then send it to serial PC with 9600 baud
Ezeuz 8:5e1854c119ba 130
Ezeuz 8:5e1854c119ba 131 // i += 90;
Ezeuz 8:5e1854c119ba 132 // if (i >= 360) {
Ezeuz 8:5e1854c119ba 133 // i = 0;
Ezeuz 6:69c59bcab6ea 134 data = lidar.GetData(i);
Ezeuz 6:69c59bcab6ea 135 speed = lidar.GetSpeed();
Ezeuz 6:69c59bcab6ea 136 intensity = lidar.GetIntensity(i);
Ezeuz 6:69c59bcab6ea 137 invalid_flag = lidar.IsDataInvalid(i);
Ezeuz 6:69c59bcab6ea 138 strength_flag = lidar.GetStrengthFlag(i);
Ezeuz 6:69c59bcab6ea 139
Ezeuz 6:69c59bcab6ea 140 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 141 //}
Ezeuz 6:69c59bcab6ea 142
Ezeuz 6:69c59bcab6ea 143 // Compass
Ezeuz 6:69c59bcab6ea 144 val = cmp.readBearing();
Ezeuz 6:69c59bcab6ea 145 mx = cmp.mag_x();
Ezeuz 6:69c59bcab6ea 146 my = cmp.mag_y();
Ezeuz 6:69c59bcab6ea 147 mz = cmp.mag_z();
Ezeuz 8:5e1854c119ba 148 lcd.printf(" %.1f", (float)val/10);
Ezeuz 6:69c59bcab6ea 149 pc.printf("%d.%d\t", val/10, val%10);
Ezeuz 6:69c59bcab6ea 150 pc.printf("%d %d %d\t", mx, my, mz);
Ezeuz 8:5e1854c119ba 151
Ezeuz 8:5e1854c119ba 152 // Standard serial management
Ezeuz 8:5e1854c119ba 153 while(pc.readable()) {
Ezeuz 8:5e1854c119ba 154 // Read serial buffer until it's empty
Ezeuz 8:5e1854c119ba 155 ser_i = pc.getc();
Ezeuz 8:5e1854c119ba 156 switch(ser_i) {
Ezeuz 8:5e1854c119ba 157 case 'r':
Ezeuz 8:5e1854c119ba 158 pc.printf("Compass Reset\n");
Ezeuz 8:5e1854c119ba 159 cmp.reset();
Ezeuz 8:5e1854c119ba 160 break;
Ezeuz 8:5e1854c119ba 161 case 'c':
Ezeuz 8:5e1854c119ba 162 pc.printf("Compass Calib\n");
Ezeuz 8:5e1854c119ba 163 fcal = !fcal;
Ezeuz 8:5e1854c119ba 164 break;
Ezeuz 9:ba07c0b8899f 165 case 'm':
Ezeuz 9:ba07c0b8899f 166 pc.printf("Motor toggle\n");
Ezeuz 9:ba07c0b8899f 167 ext = !ext;
Ezeuz 9:ba07c0b8899f 168 break;
Ezeuz 8:5e1854c119ba 169 }
Ezeuz 8:5e1854c119ba 170 // pc.putc(ser_i); // Display serial input
Ezeuz 8:5e1854c119ba 171 }
Ezeuz 6:69c59bcab6ea 172
Ezeuz 8:5e1854c119ba 173 // Standard button management
Ezeuz 8:5e1854c119ba 174 //tcal = !button.read();
Ezeuz 8:5e1854c119ba 175 //tcal = !uv_o; // Using UVTron to avoid using button
Ezeuz 8:5e1854c119ba 176 tcal = snd_o;
Ezeuz 8:5e1854c119ba 177
Ezeuz 8:5e1854c119ba 178 // cmp.reset();
Ezeuz 8:5e1854c119ba 179 if((!tcal && (tcal != ptcal)) || fcal) { // Rising or forced
Ezeuz 8:5e1854c119ba 180 if(!calib) {
Ezeuz 8:5e1854c119ba 181 cmp.startCalibrate(1);
Ezeuz 8:5e1854c119ba 182 calib = 1;
Ezeuz 8:5e1854c119ba 183 } else cmp.stopCalibrate();
Ezeuz 8:5e1854c119ba 184 fcal = 0;
Ezeuz 8:5e1854c119ba 185 }
Ezeuz 8:5e1854c119ba 186 ptcal = tcal;
Ezeuz 8:5e1854c119ba 187
Ezeuz 8:5e1854c119ba 188 pc.printf(" B%d|%d", tcal, ptcal);
Ezeuz 8:5e1854c119ba 189
Ezeuz 8:5e1854c119ba 190 lcd.printf("\n");
Ezeuz 6:69c59bcab6ea 191 pc.printf("\n");
Ezeuz 6:69c59bcab6ea 192
Ezeuz 6:69c59bcab6ea 193 wait(0.2);
Ezeuz 4:9932af380e56 194 }
dmgongora 0:79e2a8171b16 195 }