USNA WSE ES456
Emaxx Navigation code ported for MBED
Dependencies: BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed
Fork of
Emaxx_Navigation_Dynamic_HIL
by 
Emaxx Navigation Group
Diff: main.cpp
- Revision:
- 8:9817993e5df7
- Parent:
- 7:a8c2e9d049e8
- Child:
- 9:3aa9b689bda5
--- a/main.cpp Fri Dec 16 14:06:42 2016 +0000
+++ b/main.cpp Fri Apr 07 17:12:29 2017 +0000
@@ -1,22 +1,21 @@
#include "mbed.h"
#include "utilityFunctions.h"
-#include "GPSINT.h"
#include "BNO055.h"
-#include "nav_ekf.h"
#include "ServoIn.h"
#include "ServoOut.h"
-#include "NeoStrip.h"
#include "MODSERIAL.h"
#include "vehicleModel.h"
-
+#include "motor_driver.h"
#define MAX_MESSAGE_SIZE 64
#define IMU_RATE 100.0
-#define LOG_RATE 20.0
-#define GPS_RATE 1.0
+#define LOG_RATE 10.0
#define LOOP_RATE 300.0
#define CMD_TIMEOUT 1.0
-#define GEAR_RATIO (1/2.75)
+#define GEAR_RATIO (1/2.75) // gear ratio
+#define WHL_RADIUS 0.036 // Wheel radius (m)
+#define PI 3.14159
+
// Reference origin is at entrance to hospital point monument
#define REF_LAT 38.986534
#define REF_LON -76.489914
@@ -28,34 +27,29 @@
#define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
#define COURSE_MODE 1 //Commands map to heading and speed
-#define HARDWARE_MODE 0
+#define HARDWARE_MODE 0
#define HIL_MODE 1 // commands map to hardware active simulation
#define SIL_MODE 2 // commands map to software only simulation
//I2C i2c(p9, p10); // SDA, SCL
BNO055 imu(p9, p10);
-GPSINT gps(p13,p14);
-vector <int> str_buf;
int left;
// Function Prototypes
-//float saturateCmd(float cmd);
-//float wrapToPi(float ang);
+void he_callback();
int parseMessage(char * msg);
-void setLED(int *colors, float brightness);
-
// LED Definitions
DigitalOut rc_LED(LED1);
DigitalOut armed_LED(LED2);
DigitalOut auto_LED(LED3);
-DigitalOut imu_LED(LED4);
+// DigitalOut imu_LED(LED4);
+DigitalOut hall_LED(LED4);
-NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
// Comms and control object definitions
-//MODSERIAL pc(p13, p14); // tx, rx for serial USB interface to pc
+//MODSERIAL pc(p28, p27); // tx, rx for serial USB interface to pc
//MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee
MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
@@ -63,16 +57,20 @@
ServoIn CH1(p15);
ServoIn CH2(p16);
-//InterruptIn he_sensor(p11);
-
+InterruptIn he_sensor(p11);
ServoOut Steer(p22);
ServoOut Throttle(p21);
+
+
+
+
+
Timer t; // create timer instance
Ticker log_tick;
Ticker heartbeat;
float t_imu,t_gps,t_cmd,t_sim,t_ctrl;
float str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
-float psi_err,spd_err, psi_err_i,spd_err_i; // control variables
+float psi_err,spd_err, psi_err_i,spd_err_i,speed; // control variables
float t_hall, dt_hall,t_run,t_stop,t_log;
bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
float wheel_spd; // wheel speed variable
@@ -90,10 +88,16 @@
float y = 0.0; // simulation variables
float psi = 0.0; // simulation variables
float spd = 0.0; // simulation speed
+float tau = 0.25; // throttle time constant
+float Kdc = 1; // throttle dc gain
+float thr_out,thr_out_old;
+float KpWhl = 0.6;
+float dz;
+int reverse;
-nav_EKF ekf; // initialize ekf states
vehicleModel vm; //create vehicle model object
+
void rxCallback(MODSERIAL_IRQ_INFO *q)
{
MODSERIAL *serial = q->serial;
@@ -108,14 +112,14 @@
pc.baud(115200); // set baud rate of serial comm to pc
- xbee.baud(57600); // set baud rate of serial comm of wireless xbee comms
+ xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
Steer.write(1500); //Set Steer PWM to center 1000-2000 range
Throttle.write(1500); //Set Throttle to Low
xbee.attach(&rxCallback, MODSERIAL::RxIrq);
-
+
- led1=led2=led3=led4=WHITE; // set color of neo strip lights?
+ he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt
// initialize necessary float and boolean variables
left = 0;
@@ -131,8 +135,9 @@
arm_clock = 0;
auto_clock = 0;
Kp_psi = 1/1.57;
- Kp_spd = 0.3;
- Ki_spd = 0.05;
+ Kp_spd = 0.4;
+ Ki_spd = 0.2;
+ dz = 0.2;
str_cond = false;
thr_cond = false;
armed = false;
@@ -142,21 +147,20 @@
run_ctrl = false;
log_data = false;
cmd_mode = 1;
+ reverse = 0;
+
// timer and timing initializations
t.start();
t_imu = t.read();
- t_gps = t.read();
t_sim = t.read();
t_ctrl = t.read();
t_log = t.read();
t_cmd = 0;
- leds.setBrightness(0.5); // set brightness of leds
rc_LED = 0; // turn off LED 1 to indicate no RC connection
- imu_LED = 0; // turn off IMU indicator (LED 4)
- gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion
+// imu_LED = 0; // turn off IMU indicator (LED 4)
// procedure to ensure IMU is operating correctly
@@ -178,12 +182,12 @@
imu_LED = !imu_LED;
} // end while(imu.calib)
*/
- imu_LED = 1; // turns on IMU light when calibration is successful
+ //imu_LED = 1; // turns on IMU light when calibration is successful
} else {
pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly
- sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
+ //sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
/* // turn on all lights is IMU is not connected correctly
@@ -227,7 +231,7 @@
str_cond = (CH1.servoPulse > 1800); // If steering is full right
thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
- if(t.read()-auto_clock > 3) { //Auto control timeout if no commands recevied after 3 seconds
+ if(t.read()-auto_clock > 1) { //Auto control timeout if no commands recevied after 1 seconds
auto_ctrl = false;
} // end if(t.read()-autoclock>3) timeout procedure
@@ -259,46 +263,77 @@
parseMessage(buf);
newpacket = false; // reset packet flag
} // end if(newpacket)
+ float cdt = t.read()-t_ctrl;
+ t_ctrl = t.read();
if(!rc_conn) { // Is System Armed, system armed if RC not connected
// printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
+ if(auto_ctrl) {
- if(auto_ctrl) {
- float cdt = t.read()-t_ctrl;
- t_ctrl = t.read();
+
switch (cmd_mode) {
case DIRECT_MODE: {
- str = str_cmd;
- thr = thr_cmd;
- // pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
+
+ str = -str_cmd;
+
+
+ des_spd = thr_cmd;
+ spd_err = des_spd - speed;
+ spd_err_i += spd_err*cdt;
+
+ // saturate integral term
+ if(spd_err_i> 1) {
+ spd_err_i = 1;
+ }
+ if(spd_err<-1) {
+ spd_err_i = -1;
+ }
+
+ thr = Kp_spd*(des_spd-speed) + Ki_spd*spd_err_i;
+
+ if(des_spd > 0.02){
+ thr = thr + dz;
+ }
+ else if(des_spd < -0.02){
+ // speed has no sign figure out how to make it track direction using accel perhaps.
+ thr = -(dz+0.2);
+ //thr = 0.3*des_spd;
+ }else {
+ thr = 0;
+ spd_err_i = 0;
+ }
+ //thr = thr_cmd;
+ // pc.printf("des_spd %.3f, Spd %.3f Thr_cmd %.3f rev %d\r\n",des_spd,reverse*speed,thr,reverse);
// xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
break;
} // end direct mode case
case COURSE_MODE: {
if(sim_mode==0) { // if hardware is enabled use gyro and ekf
- psi_err = wrapToPi(des_psi-imu.euler.yaw);
-
- spd_err = des_spd - ekf.getSpd();
- spd_err_i += spd_err;
- str = -Kp_psi*psi_err/ekf.getSpd();
+ // psi_err = wrapToPi(des_psi-imu.euler.yaw);
+
+ // spd_err = des_spd - ekf.getSpd();
+ // spd_err_i += spd_err;
+ // str = -Kp_psi*psi_err/ekf.getSpd();
} else { // otherwise design control using simulated variables, bypass ekf states
-
- psi_err = wrapToPi(des_psi-vm.getYaw());
- str = Kp_psi*psi_err;
-
- spd_err = des_spd - vm.getSpd();
- spd_err_i += spd_err;
-
-
- /* if(spd>0.05) {
- str = Kp_psi*psi_err/vm.getSpd();
- } else {
- str = Kp_psi*psi_err/0.05;
- }*/
+
+ // psi_err = wrapToPi(des_psi-vm.getYaw());
+ // str = Kp_psi*psi_err;
+
+ // spd_err = des_spd - vm.getSpd();
+ // spd_err_i += spd_err;
+
+
+ /* if(spd>0.05) {
+ str = Kp_psi*psi_err/vm.getSpd();
+ } else {
+ str = Kp_psi*psi_err/0.05;
+ }*/
} // end if sim_mode
+
+
thr = Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;
if (thr >= 1.0) {
@@ -310,9 +345,9 @@
spd_err_i = 0.0; // Reset Integral If Saturated
} // end iff thr<0
-
- pc.printf("Psi Err: %.3f, Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f\r\n",psi_err,str,spd_err,Kp_spd,thr);
-
+
+ // pc.printf("Psi Err: %.3f, Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f rev %d\r\n",psi_err,str,spd_err,Kp_spd,thr,reverse);
+
break;
} // end COURSE_MODE case
@@ -324,13 +359,13 @@
str = saturateCmd(str);
//xbee.printf("Psi: %.3f Psi Err: %.3f Str Cmd %.3f\r\n",vm.getYaw(),psi_err,str);
if(sim_mode<2) { // only actuates if in experiment or HIL modes
- Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
- Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+ // Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
+ // Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
} else {
// won't send command to motor and servo if in SIL mode
}
-
+
} else { // goes with if auto_ctrl, manual control mode
Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
@@ -345,14 +380,24 @@
armed_LED = 0; //Turn off armed LED indicator
str = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
thr = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
-
+
if(sim_mode<2) { // if hardware is active send command to servo and throttle
- Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
- Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+ //Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
+ // Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
}
}/// end else armed
+ float thr_dot = -(1/tau)*thr_out + (Kdc/tau)*thr; // first order filter for throttle
+ thr_out = thr_out_old + thr_dot*cdt;
+ thr_out_old = thr_out;
+ if(sim_mode<2) { // if hardware is active send command to servo and throttle
+ Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
+ //Throttle.write((int)((thr_out*500.0)+1500.0)); //Write Throttle Pulse
+ mot_control(thr);
+ // pc.printf("Motcontrol: %f %f\r\n",thr,thr_out);
+ }
+
if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off
float dt = t.read()-t_imu;
if(dt > (1/IMU_RATE)) {
@@ -360,79 +405,114 @@
imu.get_accel();
imu.get_gyro();
imu.get_lia();
-
+
float tic = t.read();
- ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
- ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
t_imu = t.read();
+ // *******************************************
+ // Get Sensor Data
+ // *******************************************
+ // Compute speed from hall effect sensor
+ if(t.read()-t_hall < 0.2) {
+ speed = 0.5*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec
+ } else {
+ speed = 0;
}
- }else{ // Else we are running a simulation
+
+ if(abs(speed) <0.01){
+ if(imu.accel.x < -0.5 && thr_out < -0.05){
+ reverse = -1;
+ }else{
+ reverse = 1;
+ }
+ }
+
+
+ } // end if simmode ==0
+
+
+
+
+
+ } else { // Else we are running a simulation
float T = t.read()-t_sim;
- vm.stepModel(str,thr,T);
+ vm.stepModel(str,thr,T);
+ speed = vm.getSpd();
t_sim = t.read();
- }
-
+ }
+
if(t.read()-t_log > (1/LOG_RATE)) {
-
+
if(sim_mode > 0) {
- xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
- xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
-
- // pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
-
- // pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
- // pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+ xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+ xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+ printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+ printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+ // pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+ // pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
} else {
- xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+ //xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+ pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
- // pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
- // pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
+ // xbee.printf("Thr in %.3f Thr_out %.3f\r\n",thr,thr_out);
+ xbee.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
+ //pc.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
+
+ // pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+ //pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
}
t_log = t.read();
}
- if(t.read()-t_gps >(1/GPS_RATE)) {
- float tic2 = t.read();
- ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
- t_gps = t.read();
- }
- wait(1/LOOP_RATE);
- // status_LED=!status_LED;
- auto_ctrl_old = auto_ctrl;
+ wait(1/LOOP_RATE);
+ // status_LED=!status_LED;
+ auto_ctrl_old = auto_ctrl;
- /* } else { // else condition implies a simulation mode is enabled
+ /* } else { // else condition implies a simulation mode is enabled
- float dt = t.read()-t_imu;
- if(dt > (1/IMU_RATE)) {
+ float dt = t.read()-t_imu;
+ if(dt > (1/IMU_RATE)) {
- float tic = t.read();
+ float tic = t.read();
- x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
- y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
- psi = psi + str*dt; // turn rate kinematics
- float drag = 0.0;
- if(spd>1) {
- drag = 0.0059*spd*spd; // based on drag on a cat sized object
- } else {
- drag = 0.0059*spd;
- }
- spd = spd + (thr-drag)*dt; // give throttle offset
+ x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
+ y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
+ psi = psi + str*dt; // turn rate kinematics
+ float drag = 0.0;
+ if(spd>1) {
+ drag = 0.0059*spd*spd; // based on drag on a cat sized object
+ } else {
+ drag = 0.0059*spd;
+ }
+ spd = spd + (thr-drag)*dt; // give throttle offset
- xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
- pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
- t_imu = t.read();
- } // end if dt
+ xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
+ pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
+ t_imu = t.read();
+ } // end if dt
- } // end if sim_mode */
+ } // end if sim_mode */
} // end while(1)
} // end main
+
+
+void he_callback()
+{
+ // Hall effect speed sensor callback
+ hall_LED = !hall_LED;
+
+ dt_hall = t.read()-t_hall;
+ t_hall = t.read();
+} // end void he_callback
+
+
+
int parseMessage(char * msg)
{
@@ -457,7 +537,7 @@
default: {
}
}
- return 1;
+ return 1;
} //emd of $CMD
if(!strncmp(msg, "$PRM", 4)) {
@@ -485,7 +565,7 @@
float ref_lat = arg1;
float ref_lon = arg2;
float ref_alt = arg3;
- gps.setRefPoint(ref_lat,ref_lon,ref_alt);
+ // gps.setRefPoint(ref_lat,ref_lon,ref_alt);
pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
//xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
@@ -494,23 +574,11 @@
}
default: {
}
-
+
}
return 1;
} // end of $PRM
- if(!strncmp(msg, "$LED", 4)) {
- int arg1;
- float arg2;
- sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
- //pc.printf("%s\n",msg);
- int colors[4]= {arg1,arg1,arg1,arg1};
- float brightness = arg2;
- setLED(colors,brightness);
- return 1;
- } // end of $LED
-
-
if(!strncmp(msg, "$SIM", 4)) {
int arg1;
float arg2,arg3,arg4;
@@ -530,7 +598,7 @@
y = arg3;
psi = arg4;
vm.initialize(x,y,psi);
-
+
}
default: {
}
@@ -540,22 +608,3 @@
return 0;
}
-void setLED(int *colors,float brightness)
-{
-
- leds.setBrightness(brightness);
-
- int cidx = 0;
- int ctr = 0;
- for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
-
- if(ctr >11) {
- ctr = 0;
- cidx++;
- }
- leds.setPixel(i,colors[cidx]);
- ctr++;
- }
- leds.write();
-}
-