Diff: main.cpp

Revision:

6:a839ccbf7f9a

Parent:

3:4417217b4f66

--- a/main.cpp	Wed Feb 17 01:56:21 2016 +0000
+++ b/main.cpp	Wed Feb 17 02:43:40 2016 +0000
@@ -1,153 +1,47 @@
 //翼端can program
 #include "mbed.h"
-#include "ADXL345_I2C.h"
-#include "INA226.hpp"
-#define TO_SEND_DATAS_NUM 5
 #define INIT_SERVO_PERIOD_MS 20
 #define WAIT_LOOP_TIME 0.2
-#define CONTROL_VALUES_NUM 2
-#define TO_SEND_CAN_ID 100
 
-CAN can(p30,p29);
-CANMessage recmsg;
 Serial pc(USBTX,USBRX);
-ADXL345_I2C accelerometer(p9, p10);
-I2C ina226_i2c(p28,p27);
-INA226 VCmonitor(ina226_i2c);
 PwmOut servo1(p22);
-PwmOut servo2(p23);
+PwmOut servo2(p23);// eruron
 DigitalOut led1(LED1);
 AnalogIn a(p20);
 AnalogIn b(p19);
 
-char toSendDatas[TO_SEND_DATAS_NUM];
-char controlValues[CONTROL_VALUES_NUM];//0:eruruon,1:drug
+int neutral_a = 0;
+int neutral_b = 0;
 
-int counter = 0;
-int eruron_deg = 0;
-int drug_deg = 0;
-unsigned short ina_val;
-double V,C;
-bool SERVO_FLAG;
-bool ADXL_FLAG;
-bool INA_FLAG;
-
-int acc[3] = {0,0,0};
-
-bool servoInit(){
+bool servoInit()
+{
     servo1.period_ms(INIT_SERVO_PERIOD_MS);
     servo2.period_ms(INIT_SERVO_PERIOD_MS);
     return true;
 }
 
-bool adxlInit(){
-    accelerometer.setPowerControl(0x00);
-    accelerometer.setDataFormatControl(0x0B);
-    accelerometer.setDataRate(ADXL345_3200HZ);
-    accelerometer.setPowerControl(0x08);
-    return true;
-}
 
-bool inaInit(){
-    if(!VCmonitor.isExist()){
-        pc.printf("VCmonitor NOT FOUND\n");
-        return false;
-    }
-    ina_val = 0;
-    if(VCmonitor.rawRead(0x00,&ina_val) != 0){
-        pc.printf("VCmonitor READ ERROR\n");
-        return false;
-    }
-    VCmonitor.setCurrentCalibration();
-    return true;
-}
-
-void init(){
-   // pc.printf("Receiver\n\r");
-    SERVO_FLAG = servoInit();
-    ADXL_FLAG = adxlInit();
-    INA_FLAG = inaInit();
-}
-
-void updateDatas(){
-    if(ADXL_FLAG){
-        accelerometer.getOutput(acc);
-    }
-    if(INA_FLAG){
-        int tmp = VCmonitor.getVoltage(&V);
-        tmp = VCmonitor.getCurrent(&C);
-    }
-    for(int i = 0; i < 3; i++){
-        toSendDatas[i] = acc[i];
-        //pc.printf("%d\t",acc[i]);
-    }
-    //pc.printf("\n");
-    toSendDatas[3] = (char)(V/100);
-    toSendDatas[4] = (char)(C/0100);
-}
-
-void receiveDatas(){
-    if(can.read(recmsg)){
-        for(int i = 0; i < CONTROL_VALUES_NUM; i++){
-            controlValues[i] = recmsg.data[i];
-         //   pc.printf("%d:%d    ",i,recmsg.data[i]);
-        }
-        led1 = !led1;
-    }
-}
-
-void toString(){
-    //for(int i = 0; i < TO_SEND_DATAS_NUM; i++){
-    //    pc.printf("%i:",toSendDatas[3]);
-//        pc.printf("%i:",toSendDatas[4]);
-       pc.printf("%f:",V);
-    pc.printf("%f:",C);
-//    //}
-    //for(int i = 0; i <CONTROL_VALUES_NUM; i++){
-//      //  pc.printf("%d, ",controlValues[i]);
-//        
-//    }
-    //pc.printf("\n\r");
-//    pc.printf("%f",a.read());
-    //pc.printf("\n\r");
-}
-
-void sendDatas(){
-   if(can.write(CANMessage(TO_SEND_CAN_ID, toSendDatas, TO_SEND_DATAS_NUM))){
-    //
-      //  pc.printf("resend suc\n\r");   
-}
-}
-double calcPulse(int deg){
+double calcPulse(int deg)
+{
     return (0.00093+(deg/180.0)*(0.00235-0.00077));
 }
 
-void WriteServo(){
-    if(controlValues[0] == (char)1){
-        servo1.pulsewidth(calcPulse(90));
-    } 
-    else{
-        servo1.pulsewidth(calcPulse(45));
+void WriteServo(int a,int b)
+{
+    servoInit();
+        servo1.pulsewidth(calcPulse(a * 90));
+        servo2.pulsewidth(calcPulse(b * 90));
     }
-    if(controlValues[1] == (char)1){
-        servo2.pulsewidth(calcPulse(90));
-    } 
-    else{
-        servo2.pulsewidth(calcPulse(45));
-    }
+
     //servo1.pulsewidth(calcPulse(a.read()*170));
 //    servo2.pulsewidth(calcPulse(b.read()*170));
     //pc.printf("%f", a.read());
-}
+
 
-int main(){
-    init();
-    while(1){
-        receiveDatas();
-        WriteServo();
-           updateDatas();
-        toString();
-        sendDatas();
+int main()
+{
+    while(1) {
+        WriteServo(a,b);
         wait(WAIT_LOOP_TIME);
     }
 }
\ No newline at end of file