car_fmw: sonar support

car_cfmw/src/main.c

@@ -95,7 +95,11 @@ int main(void)
     car_conn_init();
     car_sonar_init();
 
-    sonar_prog_rotate();
-    //sonar_prog_dist_to_conn();
+    car_sonar_get_dist(0);
+	car_time_wait(3000);
+    car_sonar_get_dist(180);
+	car_time_wait(3000);
+    /* sonar_prog_rotate(); */
+    sonar_prog_dist_to_conn();
     // Also we can run usart test from here
 }

car_fmw/src/Control.kt

@@ -17,7 +17,7 @@ object Control {
         }
     }
 
-    fun executeCommand() {
+    private fun executeCommand() {
         val task = Reader.readTask()
         Leds.blink()
         Time.wait(BLINK_DURATION)
@@ -25,13 +25,14 @@ object Control {
         when (task.type.id) {
             TaskRequest.Type.DEBUG.id -> debugTask()
             TaskRequest.Type.ROUTE.id -> routeTask()
+            TaskRequest.Type.SONAR.id -> sonarTask()
         }
 
         Leds.blink()
         Time.wait(BLINK_DURATION)
     }
 
-    fun debugTask() {
+    private fun debugTask() {
         val request = Reader.readDebug()
 
         when (request.type.id) {
@@ -39,16 +40,16 @@ object Control {
         }
     }
 
-    fun routeTask() {
+    private fun routeTask() {
         val route = Reader.readRoute()
 
         val times = route.times
         val directions = route.directions
-        var j = 0
+        var i = 0
 
-        while (j < times.size) {
-            val time = times[j]
-            val direction = directions[j]
+        while (i < times.size) {
+            val time = times[i]
+            val direction = directions[i]
 
             when (direction) {
                 RouteType.FORWARD.id -> Engine.forward()
@@ -59,14 +60,29 @@ object Control {
 
             Time.wait(time)
             Engine.stop()
-            j++
+            i++
         }
 
         val response = RouteResponse.BuilderRouteResponse(0).build()
         Writer.writeRoute(response)
     }
 
-    fun sendMemoryStats() {
+    private fun sonarTask() {
+        val request = Reader.readSonar()
+        val size = request.angles.size
+        val distances = IntArray(size)
+
+        var i = 0
+        while (i < size) {
+            distances[i] = Sonar.getDistance(request.angles[i])
+            i++
+        }
+        
+        val response = SonarResponse.BuilderSonarResponse(distances).build()
+        Writer.writeSonar(response)
+    }
+
+    private fun sendMemoryStats() {
         val stats = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(
                 DebugInfo.getDynamicHeapTail(),
                 DebugInfo.getStaticHeapTail(),

car_fmw/src/ProtoRW.kt

@@ -14,7 +14,12 @@ object Reader {
         return DebugRequest.BuilderDebugRequest(DebugRequest.Type.MEMORY_STATS).parseFrom(stream).build()
     }
 
-    fun getInputStream(): CodedInputStream {
+    fun readSonar(): SonarRequest {
+        val stream = getInputStream()
+        return SonarRequest.BuilderSonarRequest(IntArray(0)).parseFrom(stream).build()
+    }
+
+    private fun getInputStream(): CodedInputStream {
         val buffer = Connection.receiveByteArray()
         return CodedInputStream(buffer)
     }
@@ -22,18 +27,24 @@ object Reader {
 
 object Writer {
     fun writeRoute(route: RouteResponse) {
-        val stream = makeOutputStream(route.getSizeNoTag())
+        val stream = getOutputStream(route.getSizeNoTag())
         route.writeTo(stream)
         Connection.sendByteArray(stream.buffer)
     }
 
     fun writeMemoryStats(stats: DebugResponseMemoryStats) {
-        val stream = makeOutputStream(stats.getSizeNoTag())
+        val stream = getOutputStream(stats.getSizeNoTag())
         stats.writeTo(stream)
         Connection.sendByteArray(stream.buffer)
     }
 
-    fun makeOutputStream(size: Int): CodedOutputStream {
+    fun writeSonar(response: SonarResponse) {
+        val stream = getOutputStream(response.getSizeNoTag())
+        response.writeTo(stream)
+        Connection.sendByteArray(stream.buffer)
+    }
+
+    private fun getOutputStream(size: Int): CodedOutputStream {
         val buffer = ByteArray(size)
         return CodedOutputStream(buffer)
     }

car_fmw/src/include/Sonar.kt

@@ -0,0 +1,11 @@
+
+external fun car_sonar_init()
+external fun car_sonar_get_dist(degree: Byte): Short
+
+object Sonar {
+    fun init() {
+        car_sonar_init()
+    }
+
+    fun getDistance(degree: Int): Int = car_sonar_get_dist(degree.toByte()).toInt()
+}

car_fmw/src/main.kt

@@ -4,6 +4,7 @@ fun main() {
     Time.init()
     Engine.init()
     Leds.init()
+    Sonar.init()
     Connection.init()
 
     /* car task */

car_fmw/src/proto/Sonar.proto

@@ -0,0 +1,10 @@
+syntax = "proto3";
+package carkot;
+
+message SonarRequest {
+    repeated int32 angles = 1;
+}
+
+message SonarResponse {
+    repeated int32 distances = 1;
+}

car_fmw/src/proto/Task.proto

@@ -8,5 +8,6 @@ message TaskRequest {
     enum Type {
         DEBUG = 0;
         ROUTE = 1;
+        SONAR = 2;
     }
 }

car_fmw/src/proto_src/Sonar.kt

@@ -0,0 +1,478 @@
+class SonarRequest private constructor (var angles: IntArray) {
+  //========== Properties ===========
+  //repeated int32 angles = 1
+
+  var errorCode: Int = 0
+
+  //========== Serialization methods ===========
+  fun writeTo (output: CodedOutputStream) {
+    //repeated int32 angles = 1
+    if (angles.size > 0) {
+      output.writeTag(1, WireType.LENGTH_DELIMITED)
+      var arrayByteSize = 0
+
+      if (angles.size != 0) {
+        do {
+          var arraySize = 0
+          var i = 0
+          while (i < angles.size) {
+            arraySize += WireFormat.getInt32SizeNoTag(angles[i])
+            i += 1
+          } 
+          arrayByteSize += arraySize
+        } while(false)
+      }
+      output.writeInt32NoTag(arrayByteSize)
+
+      do {
+        var i = 0
+        while (i < angles.size) {
+          output.writeInt32NoTag (angles[i])
+          i += 1
+        }
+      } while(false)
+    }
+
+  }
+
+  fun mergeWith (other: SonarRequest) {
+    angles = angles.plus((other.angles))
+    this.errorCode = other.errorCode
+  }
+
+  fun mergeFromWithSize (input: CodedInputStream, expectedSize: Int) {
+    val builder = SonarRequest.BuilderSonarRequest(IntArray(0))
+    mergeWith(builder.parseFromWithSize(input, expectedSize).build())
+  }
+
+  fun mergeFrom (input: CodedInputStream) {
+    val builder = SonarRequest.BuilderSonarRequest(IntArray(0))
+    mergeWith(builder.parseFrom(input).build())
+  }
+
+  //========== Size-related methods ===========
+  fun getSize(fieldNumber: Int): Int {
+    var size = 0
+    if (angles.size != 0) {
+      do {
+        var arraySize = 0
+        size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+        var i = 0
+        while (i < angles.size) {
+          arraySize += WireFormat.getInt32SizeNoTag(angles[i])
+          i += 1
+        } 
+        size += arraySize
+        size += WireFormat.getInt32SizeNoTag(arraySize)
+      } while(false)
+    }
+    size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED)
+    return size
+  }
+
+  fun getSizeNoTag(): Int {
+    var size = 0
+    if (angles.size != 0) {
+      do {
+        var arraySize = 0
+        size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+        var i = 0
+        while (i < angles.size) {
+          arraySize += WireFormat.getInt32SizeNoTag(angles[i])
+          i += 1
+        } 
+        size += arraySize
+        size += WireFormat.getInt32SizeNoTag(arraySize)
+      } while(false)
+    }
+    return size
+  }
+
+  //========== Builder ===========
+  class BuilderSonarRequest constructor (var angles: IntArray) {
+    //========== Properties ===========
+    //repeated int32 angles = 1
+    fun setAngles(value: IntArray): SonarRequest.BuilderSonarRequest {
+      angles = value
+      return this
+    }
+    fun setanglesByIndex(index: Int, value: Int): SonarRequest.BuilderSonarRequest {
+      angles[index] = value
+      return this
+    }
+
+    var errorCode: Int = 0
+
+    //========== Serialization methods ===========
+    fun writeTo (output: CodedOutputStream) {
+      //repeated int32 angles = 1
+      if (angles.size > 0) {
+        output.writeTag(1, WireType.LENGTH_DELIMITED)
+        var arrayByteSize = 0
+
+        if (angles.size != 0) {
+          do {
+            var arraySize = 0
+            var i = 0
+            while (i < angles.size) {
+              arraySize += WireFormat.getInt32SizeNoTag(angles[i])
+              i += 1
+            } 
+            arrayByteSize += arraySize
+          } while(false)
+        }
+        output.writeInt32NoTag(arrayByteSize)
+
+        do {
+          var i = 0
+          while (i < angles.size) {
+            output.writeInt32NoTag (angles[i])
+            i += 1
+          }
+        } while(false)
+      }
+
+    }
+
+    //========== Mutating methods ===========
+    fun build(): SonarRequest {
+      val res = SonarRequest(angles)
+      res.errorCode = errorCode
+      return res
+    }
+
+    fun parseFieldFrom(input: CodedInputStream): Boolean {
+      if (input.isAtEnd()) { return false }
+      val tag = input.readInt32NoTag()
+      if (tag == 0) { return false } 
+      val fieldNumber = WireFormat.getTagFieldNumber(tag)
+      val wireType = WireFormat.getTagWireType(tag)
+      when(fieldNumber) {
+        1 -> {
+          if (wireType.id != WireType.LENGTH_DELIMITED.id) {
+            errorCode = 1
+            return false
+          }
+          val expectedByteSize = input.readInt32NoTag()
+          var readSize = 0
+          var arraySize = 0
+          input.mark()
+          do {
+            var i = 0
+            while(readSize < expectedByteSize) {
+              var tmp = 0
+              tmp = input.readInt32NoTag()
+              arraySize += 1
+              readSize += WireFormat.getInt32SizeNoTag(tmp)
+            }
+          } while (false)
+          var newArray = IntArray(arraySize)
+          input.reset()
+          do {
+            var i = 0
+            while(i < arraySize) {
+              newArray[i] = input.readInt32NoTag()
+              i += 1}
+            angles = newArray
+          } while (false)
+        }
+        else -> errorCode = 4
+      }
+      return true
+    }
+
+    fun parseFromWithSize(input: CodedInputStream, expectedSize: Int): SonarRequest.BuilderSonarRequest {
+      while(getSizeNoTag() < expectedSize) {
+        parseFieldFrom(input)
+      }
+      if (getSizeNoTag() > expectedSize) { errorCode = 2 }
+      return this
+    }
+
+    fun parseFrom(input: CodedInputStream): SonarRequest.BuilderSonarRequest {
+      while(parseFieldFrom(input)) {}
+      return this
+    }
+
+    //========== Size-related methods ===========
+    fun getSize(fieldNumber: Int): Int {
+      var size = 0
+      if (angles.size != 0) {
+        do {
+          var arraySize = 0
+          size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+          var i = 0
+          while (i < angles.size) {
+            arraySize += WireFormat.getInt32SizeNoTag(angles[i])
+            i += 1
+          } 
+          size += arraySize
+          size += WireFormat.getInt32SizeNoTag(arraySize)
+        } while(false)
+      }
+      size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED)
+      return size
+    }
+
+    fun getSizeNoTag(): Int {
+      var size = 0
+      if (angles.size != 0) {
+        do {
+          var arraySize = 0
+          size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+          var i = 0
+          while (i < angles.size) {
+            arraySize += WireFormat.getInt32SizeNoTag(angles[i])
+            i += 1
+          } 
+          size += arraySize
+          size += WireFormat.getInt32SizeNoTag(arraySize)
+        } while(false)
+      }
+      return size
+    }
+
+  }
+
+}
+
+
+class SonarResponse private constructor (var distances: IntArray) {
+  //========== Properties ===========
+  //repeated int32 distances = 1
+
+  var errorCode: Int = 0
+
+  //========== Serialization methods ===========
+  fun writeTo (output: CodedOutputStream) {
+    //repeated int32 distances = 1
+    if (distances.size > 0) {
+      output.writeTag(1, WireType.LENGTH_DELIMITED)
+      var arrayByteSize = 0
+
+      if (distances.size != 0) {
+        do {
+          var arraySize = 0
+          var i = 0
+          while (i < distances.size) {
+            arraySize += WireFormat.getInt32SizeNoTag(distances[i])
+            i += 1
+          } 
+          arrayByteSize += arraySize
+        } while(false)
+      }
+      output.writeInt32NoTag(arrayByteSize)
+
+      do {
+        var i = 0
+        while (i < distances.size) {
+          output.writeInt32NoTag (distances[i])
+          i += 1
+        }
+      } while(false)
+    }
+
+  }
+
+  fun mergeWith (other: SonarResponse) {
+    distances = distances.plus((other.distances))
+    this.errorCode = other.errorCode
+  }
+
+  fun mergeFromWithSize (input: CodedInputStream, expectedSize: Int) {
+    val builder = SonarResponse.BuilderSonarResponse(IntArray(0))
+    mergeWith(builder.parseFromWithSize(input, expectedSize).build())
+  }
+
+  fun mergeFrom (input: CodedInputStream) {
+    val builder = SonarResponse.BuilderSonarResponse(IntArray(0))
+    mergeWith(builder.parseFrom(input).build())
+  }
+
+  //========== Size-related methods ===========
+  fun getSize(fieldNumber: Int): Int {
+    var size = 0
+    if (distances.size != 0) {
+      do {
+        var arraySize = 0
+        size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+        var i = 0
+        while (i < distances.size) {
+          arraySize += WireFormat.getInt32SizeNoTag(distances[i])
+          i += 1
+        } 
+        size += arraySize
+        size += WireFormat.getInt32SizeNoTag(arraySize)
+      } while(false)
+    }
+    size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED)
+    return size
+  }
+
+  fun getSizeNoTag(): Int {
+    var size = 0
+    if (distances.size != 0) {
+      do {
+        var arraySize = 0
+        size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+        var i = 0
+        while (i < distances.size) {
+          arraySize += WireFormat.getInt32SizeNoTag(distances[i])
+          i += 1
+        } 
+        size += arraySize
+        size += WireFormat.getInt32SizeNoTag(arraySize)
+      } while(false)
+    }
+    return size
+  }
+
+  //========== Builder ===========
+  class BuilderSonarResponse constructor (var distances: IntArray) {
+    //========== Properties ===========
+    //repeated int32 distances = 1
+    fun setDistances(value: IntArray): SonarResponse.BuilderSonarResponse {
+      distances = value
+      return this
+    }
+    fun setdistancesByIndex(index: Int, value: Int): SonarResponse.BuilderSonarResponse {
+      distances[index] = value
+      return this
+    }
+
+    var errorCode: Int = 0
+
+    //========== Serialization methods ===========
+    fun writeTo (output: CodedOutputStream) {
+      //repeated int32 distances = 1
+      if (distances.size > 0) {
+        output.writeTag(1, WireType.LENGTH_DELIMITED)
+        var arrayByteSize = 0
+
+        if (distances.size != 0) {
+          do {
+            var arraySize = 0
+            var i = 0
+            while (i < distances.size) {
+              arraySize += WireFormat.getInt32SizeNoTag(distances[i])
+              i += 1
+            } 
+            arrayByteSize += arraySize
+          } while(false)
+        }
+        output.writeInt32NoTag(arrayByteSize)
+
+        do {
+          var i = 0
+          while (i < distances.size) {
+            output.writeInt32NoTag (distances[i])
+            i += 1
+          }
+        } while(false)
+      }
+
+    }
+
+    //========== Mutating methods ===========
+    fun build(): SonarResponse {
+      val res = SonarResponse(distances)
+      res.errorCode = errorCode
+      return res
+    }
+
+    fun parseFieldFrom(input: CodedInputStream): Boolean {
+      if (input.isAtEnd()) { return false }
+      val tag = input.readInt32NoTag()
+      if (tag == 0) { return false } 
+      val fieldNumber = WireFormat.getTagFieldNumber(tag)
+      val wireType = WireFormat.getTagWireType(tag)
+      when(fieldNumber) {
+        1 -> {
+          if (wireType.id != WireType.LENGTH_DELIMITED.id) {
+            errorCode = 1
+            return false
+          }
+          val expectedByteSize = input.readInt32NoTag()
+          var readSize = 0
+          var arraySize = 0
+          input.mark()
+          do {
+            var i = 0
+            while(readSize < expectedByteSize) {
+              var tmp = 0
+              tmp = input.readInt32NoTag()
+              arraySize += 1
+              readSize += WireFormat.getInt32SizeNoTag(tmp)
+            }
+          } while (false)
+          var newArray = IntArray(arraySize)
+          input.reset()
+          do {
+            var i = 0
+            while(i < arraySize) {
+              newArray[i] = input.readInt32NoTag()
+              i += 1}
+            distances = newArray
+          } while (false)
+        }
+        else -> errorCode = 4
+      }
+      return true
+    }
+
+    fun parseFromWithSize(input: CodedInputStream, expectedSize: Int): SonarResponse.BuilderSonarResponse {
+      while(getSizeNoTag() < expectedSize) {
+        parseFieldFrom(input)
+      }
+      if (getSizeNoTag() > expectedSize) { errorCode = 2 }
+      return this
+    }
+
+    fun parseFrom(input: CodedInputStream): SonarResponse.BuilderSonarResponse {
+      while(parseFieldFrom(input)) {}
+      return this
+    }
+
+    //========== Size-related methods ===========
+    fun getSize(fieldNumber: Int): Int {
+      var size = 0
+      if (distances.size != 0) {
+        do {
+          var arraySize = 0
+          size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+          var i = 0
+          while (i < distances.size) {
+            arraySize += WireFormat.getInt32SizeNoTag(distances[i])
+            i += 1
+          } 
+          size += arraySize
+          size += WireFormat.getInt32SizeNoTag(arraySize)
+        } while(false)
+      }
+      size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED)
+      return size
+    }
+
+    fun getSizeNoTag(): Int {
+      var size = 0
+      if (distances.size != 0) {
+        do {
+          var arraySize = 0
+          size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED)
+          var i = 0
+          while (i < distances.size) {
+            arraySize += WireFormat.getInt32SizeNoTag(distances[i])
+            i += 1
+          } 
+          size += arraySize
+          size += WireFormat.getInt32SizeNoTag(arraySize)
+        } while(false)
+      }
+      return size
+    }
+
+  }
+
+}
+
+

car_fmw/src/proto_src/Task.kt

@@ -8,13 +8,15 @@ class TaskRequest private constructor (var type: TaskRequest.Type) {
   enum class Type(val id: Int) {
     DEBUG (0),
     ROUTE (1),
-    Unexpected(2);
+    SONAR (2),
+    Unexpected(3);
 
     companion object {
       fun fromIntToType (ord: Int): Type {
         return when (ord) {
           0 -> Type.DEBUG
           1 -> Type.ROUTE
+          2 -> Type.SONAR
           else -> Unexpected
         }
       }