From bc740b4f317c0d2a2812d1a5d4a837bebacac13c Mon Sep 17 00:00:00 2001
From: AntoineH <antoine.harle@proton.me>
Date: Wed, 4 Sep 2024 14:24:59 +0200
Subject: [PATCH] Initial motor sim

---
 src/cmd/motor/motor.go | 10 ++++--
 src/cmd/motor/serve.go | 79 ++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 85 insertions(+), 4 deletions(-)

diff --git a/src/cmd/motor/motor.go b/src/cmd/motor/motor.go
index 4c2d622..eee1c96 100644
--- a/src/cmd/motor/motor.go
+++ b/src/cmd/motor/motor.go
@@ -12,7 +12,7 @@ import (
 	"github.com/spf13/cobra"
 )
 
-type config struct {
+type Config struct {
 	Id uint16
 	Port uint16 
 	Min_pos float64
@@ -21,10 +21,16 @@ type config struct {
 	Accel float64
 }
 
+type State struct {
+	Angle float64 
+	Velocity float64
+	Error string //Error message if any
+}
+
 var(
 	motorID uint16 = 0 //Requested ID
 	ip string = "localhost" //localhost=127.0.0.1
-	curr_config config //Current config of the motor
+	curr_config Config //Current config of the motor
 )
 
 // motorCmd represents the motor command
diff --git a/src/cmd/motor/serve.go b/src/cmd/motor/serve.go
index 54ba297..c3dcc22 100644
--- a/src/cmd/motor/serve.go
+++ b/src/cmd/motor/serve.go
@@ -6,6 +6,7 @@ package motor
 
 import (
 	"fmt"
+	"time"
 
 	"github.com/spf13/cobra"
 	"github.com/spf13/viper"
@@ -17,6 +18,11 @@ import (
 	pb "github.com/AntoineHX/multi-motors-controller/src/proto"
 )
 
+var(
+	stateChan chan State // State channel of the simulation
+	cmdVelChan chan float64 // Command channel of the simulation
+)
+
 //Cobra CLI
 // serveCmd represents the serve command
 var serveCmd = &cobra.Command{
@@ -27,6 +33,7 @@ var serveCmd = &cobra.Command{
 		fmt.Println("serve called with ID ", cmd.Flag("id").Value)
 		// fmt.Println(cmd.CommandPath())
 		updateConfig()
+		init_sim()
 		serve()
 	},
 }
@@ -84,11 +91,24 @@ type server struct {
 //TODO: Fix compiling issue with google.protobuf.Empty message
 func (s *server) SetVolicty(ctx context.Context, in *pb.Velocity) (*pb.Empty, error) {
 	log.Printf("Received: %v", in.GetVelocity())
-	return &pb.Empty{}, nil
+	select{
+		case cmdVelChan<-in.GetVelocity(): //Send command
+			return &pb.Empty{}, nil
+		default: //Drop command
+			log.Printf("Failed to send command to simulation")
+			return &pb.Empty{}, nil //TODO: Return error
+	}
 }
 
 func (s *server) GetData(ctx context.Context, in *pb.Empty) (*pb.MotorData, error) {
-	return &pb.MotorData{Angle: 0, Velocity: 0, Error: ""}, nil
+	var state State
+	select {
+		case state = <-stateChan :
+			return &pb.MotorData{Angle: state.Angle, Velocity: state.Velocity, Error: state.Error}, nil
+		default:
+			log.Printf("No data available")
+			return &pb.MotorData{Angle: 0, Velocity: 0, Error: "No Data"}, nil
+	}
 }
 
 func serve() {
@@ -102,4 +122,59 @@ func serve() {
 	if err := s.Serve(lis); err != nil {
 		log.Fatalf("failed to serve: %v", err)
 	}
+}
+
+//Motor sim
+func init_sim(){
+	//Initial state
+	var state State
+	state.Angle = 0
+	state.Velocity = 0
+	state.Error = ""
+
+	//Channels //TODO: Add buffer ?
+	stateChan = make(chan State) // State channel of the simulation
+	cmdVelChan = make(chan float64)// Command channel of the simulation
+
+	go motor_sim(state, curr_config, 1) //Run sim at 10Hz
+	//close(cmdVelChan) //Stop sim
+}
+
+func motor_sim(state_init State, config Config, sim_freq float64){
+	state:=state_init 
+	cmd_vel:=state.Velocity
+	ok:=true
+	for {
+		
+		//Get command velocity from channel (non-blocking)
+		select{
+			case cmd_vel = <- cmdVelChan:
+				// check if channel is closed
+				if !ok { //Stop sim
+					log.Printf("Command channel closed. Stopping simulation...")
+					close(stateChan)
+					return
+				}
+				break;
+			default: //No command, do nothing
+		}
+		
+		// cmdVelChan
+		//Update state
+		state.Velocity = cmd_vel //Infinite acceleration
+		state.Angle += state.Velocity * 1/sim_freq //Neglect computation delay for delta time
+		
+		//TODO: Error check
+
+		//Send new state to channel (non-blocking)
+		select{
+			case stateChan <- state:
+				break;
+			default: //No message sent (buffer full/no receiver)	
+		}
+
+		log.Printf("Motor state: %+v", state)
+		//Wait for next iteration
+		time.Sleep(time.Duration(float64(time.Second)/sim_freq))
+	}
 }
\ No newline at end of file