Revision 2:ce397d92f68e, committed 2018-09-23

Comitter:

spin7ion

Date:

Sun Sep 23 12:52:12 2018 +0000

Parent:

1:e11cc38ef502

Commit message:

First sail

Changed in this revision

--- a/main.cpp	Mon Sep 10 20:11:24 2018 +0000
+++ b/main.cpp	Sun Sep 23 12:52:12 2018 +0000
@@ -1,16 +1,22 @@
 #include "mbed.h"
 #include "nRF24L01P.h"
 
-#define CHANNELS_NUMBER 12 
 #define CMD_VALUE 65535
 #define CMD_GET_STATUS 1
 #define CMD_INITIALIZE 2
 
 #define RCV_TIMEOUT_MS 1000
 
-#define SERVO_MAX 2500
+#define SERVO_MAX 2200
 #define SERVO_MIN 800
-#define SERVO_CENTER SERVO_MAX-SERVO_MIN
+#define SERVO_CENTER (SERVO_MAX-SERVO_MIN)/2+SERVO_MIN
+
+#define CHANNELS_NUMBER 12 
+#define CHANNEL_MAX 1024
+#define CHANNEL_MIN 0
+#define CHANNEL_DEPTH       3
+#define CHANNEL_YAW         1
+#define CHANNEL_THROTTLE    0
 
 #define STATE_INITILIZATION 0x00
 #define STATE_RUN 0x01
@@ -26,12 +32,11 @@
 
 uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION;
 
-/*
-PwmOut depthEsc1(PTC2);
-PwmOut depthEsc2(PTC2);
-PwmOut leftEsc(PTC2);
-PwmOut rightEsc(PTC2);
-*/
+
+PwmOut depthEsc(PA_10);
+PwmOut leftEsc(PA_8);
+PwmOut rightEsc(PA_11);
+
 
 Timer t;
 
@@ -51,21 +56,43 @@
     memset(buffer, 0, sizeof(buffer));     
 }
 
+float normToPWM(float chNorm){
+    return chNorm*(SERVO_MAX-SERVO_MIN)+SERVO_MIN;
+}
+float normChannel(uint16_t channel){
+    return ((float)channel-CHANNEL_MIN)/(CHANNEL_MAX-CHANNEL_MIN);
+}
+float chToPWM(uint16_t channel){
+    return normToPWM(normChannel(channel));
+}
+
+void channelsToEsc() {
+    if (!isInitializated()) {
+        pc.printf("WARNING!!!! NOT INITIALIZATED!!!!\r\n");
+        return;    
+    }
+    depthEsc.pulsewidth_us(chToPWM(channels[CHANNEL_DEPTH])); // servo position determined by a pulsewidth between 1-2ms
+    
+    float forwardFactor = (1.0f-normChannel(channels[CHANNEL_THROTTLE]))-0.5f;
+    float yawFactor     = normChannel(channels[CHANNEL_YAW])-0.5f;
+    pc.printf("\t\t\t\tff: %f; yf: %f\r\n", forwardFactor, yawFactor);
+    leftEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500+yawFactor*500);
+    rightEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500-yawFactor*500);
+}
+
 int main() {
-    /*
-    depthEsc1.period_us(20000);          // servo requires a 20ms period
-    depthEsc1.pulsewidth_us(SERVO_CENTER);
-    depthEsc2.period_us(20000);          // servo requires a 20ms period
-    depthEsc2.pulsewidth_us(SERVO_CENTER);
+    
+    depthEsc.period_us(20000);          // servo requires a 20ms period
+    depthEsc.pulsewidth_us(SERVO_CENTER);
     leftEsc.period_us(20000);          // servo requires a 20ms period
     leftEsc.pulsewidth_us(SERVO_CENTER);
     rightEsc.period_us(20000);          // servo requires a 20ms period
     rightEsc.pulsewidth_us(SERVO_CENTER);
     
-    */
+    
     //v_servo.pulsewidth_us(pulse); // servo position determined by a pulsewidth between 1-2ms
     
-    radio.powerUp();
+    //radio.powerUp();
     pc.baud(115200);
     pc.printf("We all live in a yellow submarine!\r\n");
     
@@ -79,6 +106,13 @@
     pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", radio.getAirDataRate() );
     pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", radio.getTxAddress() );
     pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", radio.getRxAddress() );
+    
+    /*while (radio.getRxAddress()!=0xDEADBEEF0F){
+        pc.printf("RX address fail!\r\n");
+        radio.powerDown();
+        radio.setRxAddress(0xDEADBEEF0F);
+        radio.powerUp();
+    }*/
  
     radio.setTransferSize( CHANNELS_NUMBER*sizeof(uint16_t) );
  
@@ -135,6 +169,7 @@
                 for(int i=0; i<CHANNELS_NUMBER; i++){
                     channels[i] = buffer[i];
                 }
+                channelsToEsc();
                 pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]);
             }
         }