Diff: main.cpp

Revision:

0:c7d5470d50e9

Child:

1:ec22c326aa88

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jan 15 15:30:00 2016 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+#include "ADXL345_I2C.h"
+#include "INA226.hpp"
+    
+#define BUFFER 30
+
+Serial rs485(p13,p14);
+DigitalOut Receiver(p5);
+ADXL345_I2C accelerometer(p9, p10);
+PwmOut servo1(p21);
+PwmOut servo2(p22);
+I2C i2c_Ina(p28,p27);
+INA226 Ina226(i2c_Ina);
+ 
+//ADXL345 accelerometer(p5, p6, p7, p8);
+ADXL345_I2C accelerometer(p9, p10);
+Serial pc(USBTX, USBRX);
+ 
+void ADXL345(int *x)
+{
+    x[3] = {0, 0, 0};
+     
+   // pc.printf("Starting ADXL345 test...\n");
+    //pc.printf("Device ID is: 0x%02x\n", accelerometer.getDevId());
+ 
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+ 
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+     
+    //3.2kHz data rate.
+    accelerometer.setDataRate(ADXL345_3200HZ);
+ 
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+
+        accelerometer.getOutput(x);
+         
+        //13-bit, sign extended values.
+        //pc.printf("%6i, %6i, %6i\n\r", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]);
+     
+}
+
+int INA226(double *Voltage,double *Current){
+    while(1){
+      if(Ina226.isExist() == 0){
+        Ina226.getVoltage(Voltage);
+        Ina226.getCurrent(Current);
+        }
+    //pc.printf("V : %lf, C : %lf \n\r",Voltage,Current);
+    
+    }
+    return 0;
+    }
+
+
+
+int counter = 0;
+int servo_deg1 = 0;
+int servo_deg2 = 0;
+
+int acc[3] = {0,0,0};
+
+void rs485_rx()
+{
+    int acc[3];
+    signed char rec_data = rs485.getc();
+    switch(rec_data) {
+        case 'A':
+            servo_deg1 = rs485.getc();
+           // pc.printf("counter1:%d\n\r",servo_deg1);
+            break;
+        case 'B':
+            servo_deg2 = rs485.getc();
+            //pc.printf("counter2:%d\n\r",servo_deg2);
+            break;
+        case 'P':
+        INA226_check(Voltage,Current);
+        ADXL345(acc);
+    //counter++;
+   // if(counter > 1000){
+   // counter = 0;
+   // }
+            Receiver = 1;
+            wait_ms(3);
+           // if(counter % 2 == 0){
+           rs485.putc('X');
+          // pc.printf("X");
+            rs485.putc((signed char)acc[0]);
+        //pc.printf("%5d",(signed char)acc[0]);
+            rs485.putc('Y');
+            //pc.printf("Y");
+            rs485.putc((signed char)acc[1]);
+           // pc.printf("%5d",(signed char)acc[1]);
+            rs485.putc('Z');
+            //pc.printf("Z");
+            rs485.putc((signed char)acc[2]);
+            //}
+           // else{
+           // pc.printf("%5d",(signed char)acc[2]);
+            rs485.putc('V');
+            pc.printf("V");
+            rs485.putc((signed char)Voltage);
+            pc.printf("%5d",(signed char)Voltage);
+            rs485.putc('C');
+            pc.printf("C");
+            rs485.putc((signed char)Current);
+            pc.printf("%5d\n\r",(signed char)Current);
+            //}
+            //pc.printf("\n\r");
+//            rs485.printf("X%iY%iZ%i\n\r",(short)acc[0],(short)acc[1],(short)acc[2]);
+            wait_ms(15);
+            Receiver = 0;
+            //pc.printf("x:%d,y:%d,z:%d\n\r",(signed char)acc[0],(signed char)acc[1],(signed char)acc[2]);
+            break;
+        default:
+            wait_us(5);
+          //  pc.printf("%d\n\r",rec_data);
+    }
+}
+
+void init()
+{
+    Receiver = 0;
+    pc.printf("Receiver\n\r");
+
+    rs485.baud(38400);
+    rs485.attach(rs485_rx, Serial::RxIrq);
+
+    servo1.period_ms(20);
+    servo2.period_ms(20);
+
+    accelerometer.setPowerControl(0x00);
+    accelerometer.setDataFormatControl(0x0B);
+    accelerometer.setDataRate(ADXL345_3200HZ);
+    accelerometer.setPowerControl(0x08);
+}
+
+
+int main()
+{
+    init();
+    while(1) {
+        servo1.pulsewidth(0.00093 + (servo_deg1 / 180.0) * (0.00235 - 0.00077));
+        servo2.pulsewidth(0.00093 + (servo_deg2 / 180.0) * (0.00235 - 0.00077));
+        wait_ms(10);
+    }
+}