diff --git a/src/cmd/motor/motor.go b/src/cmd/motor/motor.go index eee1c96..b1325b0 100644 --- a/src/cmd/motor/motor.go +++ b/src/cmd/motor/motor.go @@ -6,10 +6,12 @@ package motor import ( "fmt" + "log" "github.com/AntoineHX/multi-motors-controller/src/cmd" "github.com/spf13/cobra" + "github.com/spf13/viper" ) type Config struct { @@ -33,6 +35,7 @@ var( curr_config Config //Current config of the motor ) +//Cobra CLI // motorCmd represents the motor command var motorCmd = &cobra.Command{ Use: "motor", @@ -57,3 +60,32 @@ func init() { // is called directly, e.g.: // motorCmd.Flags().BoolP("toggle", "t", false, "Help message for toggle") } + +// Update curr_config from config file +func updateConfig(){ + //Find correct ID in config file + i := 0 + config_id := fmt.Sprintf("motors.%d.id",i) + for viper.IsSet(config_id){ + // log.Printf("%v / %v", viper.GetInt(config_id), int(motorID)) + if viper.GetInt(config_id) == int(motorID) { + //Extract config for this motor + err := viper.UnmarshalKey(fmt.Sprintf("motors.%d",i), &curr_config) + if err != nil { + log.Fatalf("unable to decode into struct, %v", err) + break + } + + //Sanity check + if curr_config.Id != motorID { + log.Fatalf("Failed to update config. Requested ID: %d. Got: %d.", motorID, curr_config.Id) + } + break + } + //Next motor + i++ + config_id = fmt.Sprintf("motors.%d.id",i) + } + + log.Printf("Using config: %+v", curr_config) +} \ No newline at end of file diff --git a/src/cmd/motor/moveVel.go b/src/cmd/motor/moveVel.go index d2438c8..2be8a93 100644 --- a/src/cmd/motor/moveVel.go +++ b/src/cmd/motor/moveVel.go @@ -8,8 +8,17 @@ import ( "fmt" "github.com/spf13/cobra" + + "context" + "log" + "time" + + "google.golang.org/grpc" + "google.golang.org/grpc/credentials/insecure" + pb "github.com/AntoineHX/multi-motors-controller/src/proto" ) +//Cobra CLI // moveVelCmd represents the moveVel command var moveVelCmd = &cobra.Command{ Use: "moveVel", @@ -17,6 +26,13 @@ var moveVelCmd = &cobra.Command{ Long: `moveVel command descritpion`, Run: func(cmd *cobra.Command, args []string) { fmt.Println("moveVel called with: ID ", cmd.Flag("id").Value, " Vel ", cmd.Flag("vel").Value) + updateConfig() + var vel, err = cmd.Flags().GetFloat64("vel") + if err!= nil { + log.Fatalf("Failed read requested velocity %v", err) + }else{ + moveVel(vel) + } }, } @@ -32,5 +48,25 @@ func init() { // Cobra supports local flags which will only run when this command // is called directly, e.g.: //TODO: Anonymous flag (with anonymous flag group ?) - moveVelCmd.Flags().Float32("vel", 0, "Velocity (degrees/s)") + moveVelCmd.Flags().Float64("vel", 0, "Velocity (degrees/s)") } + +//gRPC Client +func moveVel(cmd_vel float64){ + // Set up a connection to the server. + var addr = fmt.Sprintf("%s:%d", ip, curr_config.Port) //Defined in controller/serve + conn, err := grpc.NewClient(addr, grpc.WithTransportCredentials(insecure.NewCredentials())) + if err != nil { + log.Fatalf("did not connect: %v", err) + } + defer conn.Close() + c := pb.NewMotorClient(conn) + + // Contact the server and print out its response. + ctx, cancel := context.WithTimeout(context.Background(), time.Second) + defer cancel() + _, err = c.SetVelocity(ctx, &pb.Velocity{Velocity: cmd_vel}) + if err != nil { + log.Fatalf("could not send: %v", err) + } +} \ No newline at end of file diff --git a/src/cmd/motor/serve.go b/src/cmd/motor/serve.go index c3dcc22..d3749c5 100644 --- a/src/cmd/motor/serve.go +++ b/src/cmd/motor/serve.go @@ -9,7 +9,6 @@ import ( "time" "github.com/spf13/cobra" - "github.com/spf13/viper" "context" "log" @@ -54,42 +53,14 @@ func init() { //TODO: Support Motor ID flag } -func updateConfig(){ - //Find correct ID in config file - i := 0 - config_id := fmt.Sprintf("motors.%d.id",i) - for viper.IsSet(config_id){ - // log.Printf("%v / %v", viper.GetInt(config_id), int(motorID)) - if viper.GetInt(config_id) == int(motorID) { - //Extract config for this motor - err := viper.UnmarshalKey(fmt.Sprintf("motors.%d",i), &curr_config) - if err != nil { - log.Fatalf("unable to decode into struct, %v", err) - break - } - - //Sanity check - if curr_config.Id != motorID { - log.Fatalf("Failed to update config. Requested ID: %d. Got: %d.", motorID, curr_config.Id) - } - break - } - //Next motor - i++ - config_id = fmt.Sprintf("motors.%d.id",i) - } - - log.Printf("Using config: %+v", curr_config) -} - //gRPC server -// server is used to implement MotorsControllerServer. +// server is used to implement MotorServer. type server struct { - pb.UnimplementedMotorsControllerServer + pb.UnimplementedMotorServer } //TODO: Fix compiling issue with google.protobuf.Empty message -func (s *server) SetVolicty(ctx context.Context, in *pb.Velocity) (*pb.Empty, error) { +func (s *server) SetVelocity(ctx context.Context, in *pb.Velocity) (*pb.Empty, error) { log.Printf("Received: %v", in.GetVelocity()) select{ case cmdVelChan<-in.GetVelocity(): //Send command @@ -117,7 +88,7 @@ func serve() { log.Fatalf("failed to listen: %v", err) } s := grpc.NewServer() - pb.RegisterMotorsControllerServer(s, &server{}) + pb.RegisterMotorServer(s, &server{}) log.Printf("server listening at %v", lis.Addr()) if err := s.Serve(lis); err != nil { log.Fatalf("failed to serve: %v", err) @@ -132,9 +103,9 @@ func init_sim(){ 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 + //Channels + stateChan = make(chan State, 1) // State channel of the simulation + cmdVelChan = make(chan float64, 1)// Command channel of the simulation go motor_sim(state, curr_config, 1) //Run sim at 10Hz //close(cmdVelChan) //Stop sim diff --git a/src/proto/motorsim.proto b/src/proto/motorsim.proto index 657578b..eb9fb5c 100644 --- a/src/proto/motorsim.proto +++ b/src/proto/motorsim.proto @@ -13,7 +13,7 @@ service MotorsController { } // Server converting target angles to velocity profile for mo service Motor { - rpc SetVolicty(Velocity) returns (Empty) {} + rpc SetVelocity(Velocity) returns (Empty) {} rpc GetData(Empty) returns (MotorData) {} }