Diff: main.cpp

Revision:

12:1e3227a6fcd7

Parent:

10:8722053fb75c

Child:

13:0c5942931cad

--- a/main.cpp	Fri Feb 16 12:45:25 2018 +0000
+++ b/main.cpp	Sun Dec 16 01:53:07 2018 +0000
@@ -1,195 +1,60 @@
-/*****************************************************
-- Description: mbed to Dynamixel connection test using
-    the library
-- Requirements: 
-    Dynamixel (i.e. DX116, RX28)
-    MAX3088/MAX485 (RS485 transceiver)
-- Connections:
-    MAX3088     --  mbed
-    ======================
-    Pin 1       --  Pin 14
-    Pin 2       --  Pin 15
-    Pin 4       --  Pin 13
-    
-- Comments:
-    See schematic for wiring details and class 
-    documentation for available methods.
-*****************************************************/
+#include "mbed.h"
+#include "Stepper.h"
+#include "AX12.h"
+#include "Uvtron.h"
+#include "TPA81new.h"
 
-#include "mbed.h"
-#include "Dynamixel.h"
-#include "TextLCD.h"
-#include "Uvtron.h"
-
-#include "CMPS11.h"
-#include "LIDAR.h"
+#define WAIT_TIME 0.02
 
-// Defines
-#define IR_CONST 1.229
-
-// Settings
-TextLCD lcd(PA_5, PA_11, PA_6, PB_12, PA_7, PB_6);  // LCD          : rs, e, d4-d7
-AnalogIn ir(PC_5);                                  // Sharp IR     : analog
-DigitalOut m2(PA_15);                               // extinguisher : 12V out M2, possibly broken
-DigitalOut m1(PB_7);                                // extinguisher : 12V out M1
-DigitalOut led1(PC_13);                             // GPIO high is 3V3
-DigitalOut led2(PC_14);
-DigitalIn sound(PA_12);                             // Sound act    : digital, active low
-DigitalIn uv(PB_8);                                 // UVTron       : digital, pin 2 = active low
-Serial pc(USBTX, USBRX);                            // tx, rx
+Serial pc(USBTX, USBRX);
+// stepper(PinName _en, PinName ms1, PinName ms2, PinName ms3, PinName _stepPin, PinName dir);
+stepper s(PC_6, PC_8, PA_12, PC_7, PA_11, PB_12);
+Uvtron uv(PC_12);
 
-AnalogIn line1(PB_1);                               // Line sensor  : analog (or digital)
-AnalogIn line2(PA_0);                               // Line sensor  : analog (or digital)
-
-LIDAR lidar (PC_10, PC_11, PA_1);                   // Lidar        : tx,rx, motor PWM
-CMPS11 cmp(PB_4, PA_8, 0xC0);                       // Compass      : I2C3_SDA, I2C3_SCL, Addr
-
-DigitalIn button(USER_BUTTON);                      // Button
+AX12 servo1(PC_10, PC_11, PA_13, 6,  1000000); // tx, rx, tx_enable, servo ID, baud rate  
+AX12 servo2(PC_10, PC_11, PA_13, 22, 1000000);
+AX12 servo3(PC_10, PC_11, PA_13, 19, 1000000);
 
-/* About interrupt
-Where are the interrupt pins on NUCLEO-F411RE?
-
-If you're using a recent version of the mbed lib (right mouse button, update in the online compiler):
-Every unique numbered pin. That means you can use any pin as InterruptIn, but you cannot use multiple
-pins with the same number on a different port as InterruptIn. So you can use PA_1, PB_2, PA_3, PC_4, etc.
-But in this example you could not use also PE_1. */
+TPA81 tpay(PB_9, PB_8, 0xDE);
+DigitalOut led(LED1);
 
 int main()
-{   
-    // Servos
-    for (int i = 1; i <= 18; i++) {
-        Dynamixel servo(PC_6, PC_7, PC_4, i, 1000000);  // Servo comm   : tx, rx, txEn, id, baud
-        servo.setSpeed(100);
-        servo.move(512);        // Midddle, thus 90 deg position
-    }
-    
-    float line1_o = 0;
-    float line2_o = 0;
-    float ir_o = 0;
-    float snd_o = 0;
-    float uv_o = 0;
-    int ext = 0;        // Extinguisher state
-    
-    int tcal = 0;   // Toggle calibration
-    int fcal = 0;   // Force calibration
-    int ptcal = 0;  // Prev toggle calibration
-    int calib = 0;  // Calibration state
-    
-    char ser_i;     // Get data from serial input buffer
-    
-    // Lidar
-    lidar.StartData();
-    int i = 0;
-    float data, speed;
-    short intensity;
-    bool invalid_flag, strength_flag;
-    
-    // Compass
-    int val;
-    int16_t mx,my,mz;
-    cmp.reset();
-    
-    // Motor switching test
-    m2 = 0;
-    m1 = 0;
-    
-    wait(1);
-    
-    while (1) {
-        // LCD
-        lcd.printf("%.1f L%.2f|%.2f\n", ir_o, line1_o, line2_o);
-        pc.printf("%.2fcm L%.2f|%.2f M%d ", ir_o, line1_o, line2_o, ext);
-
-        lcd.printf("s%d u%d", (int)snd_o, (int)uv_o);
-        pc.printf("s%.2f u%.2f ", snd_o, uv_o);
-        
-        // IR
-        ir_o = ((1-ir.read())*26+4);  // Convert and read the analog input value (value from 0.0 to 1.0)
-        
-        // Line Sensor
-        line1_o = line1.read();
-        line2_o = line2.read();
+{
+    s.enable();
+    while (1)
+    {
         
-        // Extinguisher (12V output)
-        m2 = ext;
-        m1 = ext;
-        
-        // LED
-        led1 = 1;
-        led2 = 1;
-        
-        // Sound Activator
-        snd_o = sound.read();
-        
-        // UV
-        uv_o = uv.read();
-        
-        // Lidar
-        // Aquire LIDAR data from angle 0, 45, 90, 135... 315
-        // Then send it to serial PC with 9600 baud
-        
-        // i += 90;
-        // if (i >= 360) {
-        //    i = 0;
-        data = lidar.GetData(i);
-        speed = lidar.GetSpeed();
-        intensity = lidar.GetIntensity(i);
-        invalid_flag = lidar.IsDataInvalid(i);
-        strength_flag = lidar.GetStrengthFlag(i);
-        
-        pc.printf("Spd=%.1f; Sdt=%d; D=%.1f; I=%.1d; finvalid=%d; fstrength=%d\t", speed, i, data, intensity, invalid_flag, strength_flag);
-        //}
-        
-        // Compass
-        val = cmp.readBearing();
-        mx = cmp.mag_x();
-        my = cmp.mag_y();
-        mz = cmp.mag_z();
-        lcd.printf(" %.1f", (float)val/10);
-        pc.printf("%d.%d\t", val/10, val%10);
-        pc.printf("%d %d %d\t", mx, my, mz);
-        
-        // Standard serial management
-        while(pc.readable()) {
-            // Read serial buffer until it's empty
-            ser_i = pc.getc();
-            switch(ser_i) {
-                case 'r':
-                    pc.printf("Compass Reset\n");
-                    cmp.reset();
-                    break;
-                case 'c':
-                    pc.printf("Compass Calib\n");
-                    fcal = !fcal;
-                    break;
-                case 'm':
-                    pc.printf("Motor toggle\n");
-                    ext = !ext;
-                    break;
-            }
-            // pc.putc(ser_i);  // Display serial input
+        // TPA
+        pc.printf("%d", tpay.getTemp(0));
+        int i;
+        pc.printf("\nTPA Y \n");
+        tpay.Read();
+        for (i=2; i<=9; i++) {
+            pc.printf("%d ",tpay.getTemp(i));
         }
         
-        // Standard button management
-        //tcal = !button.read();
-        //tcal = !uv_o;   // Using UVTron to avoid using button
-        tcal = snd_o;
+        // Stepper
+        pc.printf("Stepper\n");
+        s.step(1, 1, 1/WAIT_TIME);
         
-        // cmp.reset();
-        if((!tcal && (tcal != ptcal)) || fcal) {  // Rising or forced
-            if(!calib) {
-                cmp.startCalibrate(1);
-                calib = 1;
-            } else cmp.stopCalibrate();
-            fcal = 0;
-        }
-        ptcal = tcal;
+        // Servo
+        servo1.SetGoal(240);
+        servo2.SetGoal(240);
+        servo3.SetGoal(240);
+        pc.printf("Servo : 240\n");
+        led = 1;
+        wait(1);
+        servo1.SetGoal(150);
+        servo2.SetGoal(150);
+        servo3.SetGoal(150);
+        pc.printf("Servo : 150\n");
+        led = 0;
+        wait(1);
         
-        pc.printf(" B%d|%d", tcal, ptcal);
-        
-        lcd.printf("\n");
-        pc.printf("\n");
-        
-        wait(0.2);
-    }
-}   
\ No newline at end of file
+        // Uvtron
+        uv.Read();
+        int read = uv.Flag;
+        if (read) pc.printf("FIRE DETECTED\n");
+        else pc.printf("NOT DETECTED\n");
+    }    
+}
\ No newline at end of file