change steer and speed to speedl, speedr

This commit is contained in:
interfisch 2019-06-14 01:04:39 +02:00
parent 11274864ab
commit a2b6f50582
1 changed files with 23 additions and 11 deletions

View File

@ -41,8 +41,10 @@ int cmd2;
int cmd3;
typedef struct{
int16_t steer;
int16_t speed;
//int16_t steer;
//int16_t speed;
int16_t speedl;
int16_t speedr;
//uint32_t crc;
uint8_t checksum; //simple checksum for error handling and connection break check. 0 disables motors
} Serialcommand;
@ -51,8 +53,11 @@ volatile Serialcommand command;
uint8_t button1, button2;
int steer; // global variable for steering. -1000 to 1000
int speed; // global variable for speed. -1000 to 1000
//int steer; // global variable for steering. -1000 to 1000
//int speed; // global variable for speed. -1000 to 1000
int speedLfiltering; //speed left -1000 to 1000, used for filtering
int speedRfiltering; //speed right -1000 to 1000, used for filtering
int speed; //not used for actual control
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
@ -220,7 +225,7 @@ int main(void) {
#endif
#ifdef CONTROL_SERIAL_USART2
uint8_t calcchecksum = ((uint8_t) ((uint8_t)command.steer)*((uint8_t)command.speed)); //simple checksum
uint8_t calcchecksum = ((uint8_t) ((uint8_t)command.speedl)*((uint8_t)command.speedr)); //simple checksum
if (calcchecksum==0 || calcchecksum==255){ calcchecksum=1; } //cannot be 0 or 255 (special purpose)
if (command.checksum==0 || command.checksum==calcchecksum){ //motor off or correct checksum
@ -228,8 +233,8 @@ int main(void) {
timeout = 0; //values ok, reset timeout
}
cmd1 = CLAMP((int16_t)command.steer, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speed, -1000, 1000);
cmd1 = CLAMP((int16_t)command.speedl, -1000, 1000);
cmd2 = CLAMP((int16_t)command.speedr, -1000, 1000);
if (command.checksum==0){ //received checksum 0 disables motors
enable=0;
}else{ //checksum ok and not intended to disable motors
@ -248,13 +253,20 @@ int main(void) {
#endif
// ####### LOW-PASS FILTER #######
steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
//steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
//speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
speedLfiltering = speedLfiltering * (1.0 - FILTER) + cmd1 * FILTER;
speedRfiltering = speedRfiltering * (1.0 - FILTER) + cmd2 * FILTER;
// ####### MIXER #######
speedR = CLAMP(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT, -1000, 1000);
speedL = CLAMP(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT, -1000, 1000);
//speedR = CLAMP(speed * SPEED_COEFFICIENT - steer * STEER_COEFFICIENT, -1000, 1000);
//speedL = CLAMP(speed * SPEED_COEFFICIENT + steer * STEER_COEFFICIENT, -1000, 1000);
speedL=CLAMP(speedLfiltering, -1000, 1000); //apply motorspeed
speedR=CLAMP(speedRfiltering, -1000, 1000); //apply motorspeed
speed = (speedL+speedR)/2; //calculate speed. needed for some checks
#ifdef ADDITIONAL_CODE