├── BLDC_ESP32_A.ino ├── BLDC_ESP32_CAN_LITE.ino ├── BLDC_ESP32_CAN_Master.ino ├── BLDC_ESP32_b.ino ├── BLDC_ESP32_now.ino ├── Brushless ESP-32.pdf ├── CAN_TEST.ino ├── QUAD_BRAIN_CAN.ino ├── QUAD_BRAIN_CAN_TEST.ino ├── QUAD_BRAIN_T0.ino └── esp-now-had.ino /BLDC_ESP32_A.ino: -------------------------------------------------------------------------------- 1 | //include 2 | 3 | 4 | #include 5 | #include "driver/mcpwm.h" 6 | #include "soc/mcpwm_reg.h" 7 | #include "soc/mcpwm_struct.h" 8 | #include "driver/periph_ctrl.h" 9 | #include "driver/spi_master.h" 10 | #include "soc/gpio_sig_map.h" 11 | #include "soc/spi_reg.h" 12 | #include "soc/dport_reg.h" 13 | #include "soc/spi_struct.h" 14 | #include "soc/sens_reg.h" 15 | #include "soc/sens_struct.h" 16 | //#include "esp_adc_cal.h" 17 | #include 18 | 19 | #include "BluetoothSerial.h" 20 | 21 | #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) 22 | #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it 23 | #endif 24 | 25 | BluetoothSerial SerialBT; 26 | 27 | //#include "wiring_private.h" // pinPeripheral() function 28 | 29 | // I2c OLED /////////////////////////////////////////////////////////// 30 | #include "ssd1306.h" // library by Alexey Dynda 31 | char display_str[] = "1234567890"; 32 | #include "EEPROM.h" 33 | 34 | int addr = 0; 35 | #define EEPROM_SIZE 64 36 | 37 | //GPIO.out_w1tc and GPIO.out_w1ts 38 | 39 | int count; 40 | 41 | static const int spiClk = 2000000; // 1 MHz 42 | SPIClass * hspi = NULL; 43 | 44 | 45 | hw_timer_t * timer = NULL; 46 | portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; 47 | int hi_byte=0; 48 | int lo_byte=0; 49 | int encoder_int =0; 50 | int encoder_ext = 0; 51 | int drv_spi_value = 0; 52 | 53 | /////////////////////////////////////////////////////////////////////////////////////// 54 | int man_offset=180; 55 | #define SIN_ARRAY_SIZE_BITS 12 56 | #define SIN_ARRAY_SIZE (1<begin(); 168 | pinMode(INT_ENC_nCS, OUTPUT); 169 | pinMode(EXT_ENC_nCS, OUTPUT); 170 | pinMode(DRV_nSCS, OUTPUT); 171 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 172 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 173 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 174 | hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE1)); //AS5047 175 | //hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE3)); //MA702 176 | SPI2.mosi_dlen.usr_mosi_dbitlen = (1 * 16) - 1; 177 | SPI2.miso_dlen.usr_miso_dbitlen = (1 * 16) - 1; 178 | } 179 | 180 | int even=0; 181 | void IRAM_ATTR SPI_Read_Encoder(){ 182 | GPIO.out_w1tc = ((uint32_t)1 << INT_ENC_nCS); 183 | SPI2.data_buf[0] = 0xFFFF; //AS5147 184 | //SPI2.data_buf[0] = 0x0000; //MA702 185 | SPI2.cmd.usr = 1; 186 | while(SPI2.cmd.usr); 187 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 188 | encoder_int = SPI2.data_buf[0] & 0xFFFF; 189 | if(!SPI2.ctrl.rd_bit_order){ 190 | encoder_int = (encoder_int >> 8) | (encoder_int << 8); 191 | } 192 | encoder_int = encoder_int & 0x3FFF ; 193 | 194 | if (even){ 195 | GPIO.out_w1tc = ((uint32_t)1 << EXT_ENC_nCS); 196 | SPI2.data_buf[0] = 0xFFFF; //AS5147 197 | //SPI2.data_buf[0] = 0x0000; //MA702 198 | SPI2.cmd.usr = 1; 199 | while(SPI2.cmd.usr); 200 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 201 | encoder_ext = SPI2.data_buf[0] & 0xFFFF; 202 | if(!SPI2.ctrl.rd_bit_order){ 203 | encoder_ext = (encoder_ext >> 8) | (encoder_ext << 8); 204 | } 205 | encoder_ext = encoder_ext & 0x3FFF ; 206 | even=0; 207 | } 208 | else 209 | { 210 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 211 | SPI2.data_buf[0] = 0xFFFF; //AS5147 212 | //SPI2.data_buf[0] = 0x0000; //MA702 213 | SPI2.cmd.usr = 1; 214 | while(SPI2.cmd.usr); 215 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 216 | drv_spi_value = SPI2.data_buf[0] & 0xFFFF; 217 | if(!SPI2.ctrl.rd_bit_order){ 218 | drv_spi_value = (drv_spi_value >> 8) | (drv_spi_value << 8); 219 | } 220 | //drv_spi_value 221 | even=1; 222 | } 223 | } 224 | 225 | 226 | 227 | void IRAM_ATTR Motor_CTRL(void) 228 | { 229 | Motor_Position_Old = Motor_Position; 230 | Mag_Sensor_Data = encoder_int; 231 | Motor_Position = (Mag_Sensor_Data>>2); //& 0x2FFF; 232 | 233 | if (start<2) 234 | { 235 | start++; 236 | Motor_Abs = 0; 237 | } 238 | Motor_Pos_Change = (Motor_Position-Motor_Position_Old); 239 | 240 | if (Motor_Pos_Change>2048) 241 | off_set = Motor_Pos_Change-4096; 242 | else if (Motor_Pos_Change<-2048) 243 | off_set = Motor_Pos_Change + 4096; 244 | else 245 | off_set = Motor_Pos_Change; 246 | 247 | Motor_Abs += off_set; 248 | 249 | Joint_Position = Motor_Abs; 250 | 251 | // PID Control - Just P for now 252 | Motor_Error = (Joint_Position - Tar_Position); 253 | Motor_Joint = -Kp * Motor_Error; 254 | /* 255 | if (forward) 256 | Motor_Joint = 250; 257 | else 258 | Motor_Joint = -250; 259 | */ 260 | 261 | if (Motor_Joint > 0) // Forward or Reverse 262 | { 263 | drive = PHASE_OFFSET; //phase_set; 264 | } 265 | else 266 | { 267 | drive = -PHASE_OFFSET; //phase_set; 268 | Motor_Joint = -Motor_Joint; 269 | } 270 | // Limit Voltage/PWM to motor 271 | if (Motor_Joint > MAX_PWM) 272 | Motor_Joint = MAX_PWM; 273 | 274 | // Drive Motor at ~90deg electrically ahead of magnets 275 | Target_Phase = Motor_Position + drive + man_offset;//MAGNET_OFFSET; // MAGNET_OFFSET; 276 | 277 | //Handle wrap-around 278 | if (Target_Phase >= SIN_ARRAY_SIZE) 279 | Target_Phase = Target_Phase - SIN_ARRAY_SIZE; 280 | else if (Target_Phase < 0) 281 | Target_Phase = Target_Phase + SIN_ARRAY_SIZE; 282 | else 283 | {} 284 | } 285 | 286 | /* 287 | * 288 | */ 289 | void IRAM_ATTR Motor_Vector_Phases(int Mot_Phase, int Motor_PWM) 290 | { 291 | int U_Ang = Mot_Phase; 292 | int V_Ang = Mot_Phase + phase_120; 293 | int W_Ang = Mot_Phase - phase_120; 294 | 295 | if (V_Ang >= SIN_ARRAY_SIZE) 296 | V_Ang -=SIN_ARRAY_SIZE; 297 | 298 | if (W_Ang < 0) 299 | W_Ang +=SIN_ARRAY_SIZE; 300 | 301 | PWMUU = (SinArray[U_Ang] * Motor_PWM) >>FULL_PWM_BITS; 302 | PWMVV = (SinArray[V_Ang] * Motor_PWM) >>FULL_PWM_BITS; 303 | PWMWW = (SinArray[W_Ang] * Motor_PWM) >>FULL_PWM_BITS; 304 | } 305 | 306 | void IRAM_ATTR Three_Phases(void) 307 | { 308 | if (Motor_on==0) 309 | { 310 | PWMUU = 0; 311 | PWMVV = 0; 312 | PWMWW = 0; 313 | } 314 | MCPWM0.channel[0].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMUU; 315 | MCPWM0.channel[1].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMVV; 316 | MCPWM0.channel[2].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMWW; 317 | } 318 | /* 319 | * 320 | */ 321 | void IRAM_ATTR Setup_Sin_array(void) 322 | { 323 | int step1; 324 | float tempa; 325 | float tempb; 326 | int phase; 327 | sin_scale = 1.0/((sin(60*DEG_RAD))+(sin(60*DEG_RAD))); 328 | phase_120 = SIN_ARRAY_SIZE/(MOTOR_POLE_PAIRS*3); 329 | for (step1=0; step1> 8) | (drv_spi_value << 8); 668 | } 669 | return(drv_spi_value & 0xFFFF); 670 | } 671 | 672 | unsigned int DRV8305_SPI_Write(unsigned int reg, unsigned int data_write) 673 | { 674 | unsigned int drv_spi_value; 675 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 676 | SPI2.data_buf[0] = (reg<<3) + ((data_write & 0x1F00)>>8) + ((data_write & 0x00FF)<<8); 677 | SPI2.cmd.usr = 1; 678 | while(SPI2.cmd.usr); 679 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 680 | drv_spi_value = SPI2.data_buf[0] & 0xFFFF; 681 | if(!SPI2.ctrl.rd_bit_order){ 682 | drv_spi_value = (drv_spi_value >> 8) | (drv_spi_value << 8); 683 | } 684 | return(drv_spi_value); 685 | } 686 | 687 | int IRAM_ATTR local_adc1_read(int channel) { 688 | uint16_t adc_value; 689 | SENS.sar_meas_start1.sar1_en_pad = (1 << channel); // only one channel is selected 690 | while (SENS.sar_slave_addr1.meas_status != 0); 691 | SENS.sar_meas_start1.meas1_start_sar = 0; 692 | SENS.sar_meas_start1.meas1_start_sar = 1; 693 | while (SENS.sar_meas_start1.meas1_done_sar == 0); 694 | adc_value = SENS.sar_meas_start1.meas1_data_sar; 695 | return adc_value; 696 | } 697 | int voltage_value; 698 | 699 | #define voltage_scale (1.0/18.16) 700 | void IRAM_ATTR loop() { 701 | int i; 702 | unsigned int temp; 703 | char ser_read; 704 | count++; 705 | if (count>99) 706 | count=0; 707 | 708 | 709 | voltage_value = analogRead(VOLT_MON); 710 | voltage = (float)voltage_value * voltage_scale; 711 | textDemo(); 712 | //delay(10); 713 | //CAN_Send(); 714 | 715 | 716 | 717 | 718 | //delay(50); 719 | 720 | GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 721 | 722 | //analogRead(VOLT_MON); 723 | if (SerialBT.hasClient()) 724 | Serial.print ("BT "); 725 | else 726 | Serial.print ("OFF "); 727 | 728 | Serial.print (man_offset); 729 | Serial.print (" "); 730 | Serial.println (analogRead(VOLT_MON)); 731 | //Serial.println (local_adc1_read(DRV_SO1)); 732 | /* 733 | Serial.print (" "); 734 | Serial.print (analogRead(local_adc1_read(DRV_SO2))); 735 | Serial.print (" "); 736 | Serial.print (analogRead(local_adc1_read(DRV_SO3))); 737 | Serial.print (" "); 738 | Serial.print (analogRead(local_adc1_read(VOLT_MON))); 739 | Serial.print (" "); 740 | Serial.print (analogRead(local_adc1_read(MOTOR_TEMP))); 741 | Serial.print (" "); 742 | Serial.println (analogRead(local_adc1_read(VOLT_MON))); 743 | */ 744 | 745 | GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 746 | //GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 747 | //analogRead(DRV_SO2); 748 | //GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 749 | // GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 750 | //analogRead(DRV_SO3); 751 | //GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 752 | //SerialBT.print("@ "); 753 | /* 754 | if (Serial.available()) 755 | { 756 | ser_read = Serial.read(); 757 | } 758 | */ 759 | 760 | if (SerialBT.available()) 761 | { 762 | //ser_read = SerialBT.read(); 763 | ser_read = SerialBT.read(); 764 | SerialBT.println(ser_read); 765 | if (ser_read=='l') 766 | { 767 | man_offset++; 768 | } 769 | else if(ser_read=='q') 770 | { 771 | Motor_on=0; 772 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 773 | } 774 | else if(ser_read=='w') 775 | { 776 | Motor_on=1; 777 | GPIO.out_w1ts= ((uint32_t)1 << DRV_EN_GATE); 778 | } 779 | else if(ser_read=='k') 780 | { 781 | man_offset--; 782 | } 783 | else if (ser_read=='b') 784 | { 785 | Motor_Abs=0; 786 | } 787 | 788 | else if (ser_read=='a') 789 | { 790 | Tar_Position=-8000; 791 | //forward=0; 792 | } 793 | else if (ser_read=='s') 794 | { 795 | Tar_Position=0; 796 | //forward=1; 797 | } 798 | else if (ser_read=='d') 799 | { 800 | Tar_Position=8000; 801 | //forward=1; 802 | } 803 | else if (ser_read=='f') 804 | { 805 | fault=0; 806 | delay(10); 807 | for (i=0; i<15;i++) 808 | { 809 | temp=DRV8305_SPI_Read(i); 810 | Serial.print(i); 811 | Serial.print(" "); 812 | Serial.println(temp, HEX); 813 | } 814 | //Serial4_DRV_Write(7, 0x296); 815 | fault=1; 816 | } 817 | 818 | } 819 | 820 | } 821 | -------------------------------------------------------------------------------- /BLDC_ESP32_CAN_LITE.ino: -------------------------------------------------------------------------------- 1 | //#define MINI 1 2 | 3 | #include 4 | #include "driver/mcpwm.h" 5 | #include "soc/mcpwm_reg.h" 6 | #include "soc/mcpwm_struct.h" 7 | #include "driver/periph_ctrl.h" 8 | #include "driver/spi_master.h" 9 | #include "soc/gpio_sig_map.h" 10 | #include "soc/spi_reg.h" 11 | #include "soc/dport_reg.h" 12 | #include "soc/spi_struct.h" 13 | #include "soc/sens_reg.h" 14 | #include "soc/sens_struct.h" 15 | #include "esp_adc_cal.h" 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #define LED_PIN (2) 22 | 23 | int32_t other_motor_pos; 24 | 25 | //#include "wiring_private.h" // pinPeripheral() function 26 | 27 | int current_config = 0; 28 | #include "EEPROM.h" 29 | 30 | int addr = 0; 31 | #define EEPROM_SIZE 16 32 | int count; 33 | byte eeprom_data[EEPROM_SIZE]; 34 | 35 | #define EEPROM_SIZE 16 36 | #define CONFIG_ARRAY_SIZE 6 37 | int16_t config_array[CONFIG_ARRAY_SIZE]; 38 | 39 | #define ID_OS 0 40 | #define MAG_OFFSET_OS 1 41 | #define PHASE_OFFSET_OS 2 42 | #define TEMP_MAX_OS 3 43 | #define KP_OS 4 44 | #define REV_OS 5 45 | //GPIO.out_w1tc and GPIO.out_w1ts 46 | 47 | static const int spiClk = 2000000; // 1 MHz 48 | SPIClass * hspi = NULL; 49 | 50 | 51 | hw_timer_t * timer = NULL; 52 | portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; 53 | int hi_byte=0; 54 | int lo_byte=0; 55 | int encoder_int =0; 56 | int encoder_ext = 0; 57 | //int drv_spi_value = 0; 58 | 59 | /////////////////////////////////////////////////////////////////////////////////////// 60 | int man_offset=142; 61 | #define SIN_ARRAY_SIZE_BITS 12 62 | #define SIN_ARRAY_SIZE (1<begin(SPI_SCLK,SPI_MISO, SPI_MOSI,0); 241 | pinMode(INT_ENC_nCS, OUTPUT); 242 | #if MINI 243 | #else 244 | pinMode(EXT_ENC_nCS, OUTPUT); 245 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 246 | #endif 247 | pinMode(DRV_nSCS, OUTPUT); 248 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 249 | 250 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 251 | hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE3)); //MA702 252 | 253 | SPI2.mosi_dlen.usr_mosi_dbitlen = (1 * 8) - 1; 254 | SPI2.miso_dlen.usr_miso_dbitlen = (1 * 8) - 1; 255 | } 256 | 257 | int even=0; 258 | void IRAM_ATTR SPI_Read_Encoder(){ 259 | 260 | GPIO.out_w1tc = ((uint32_t)1 << INT_ENC_nCS); 261 | encoder_int = SPI2.data_buf[0]; 262 | SPI2.data_buf[0] = 0; 263 | SPI2.cmd.usr = 1; 264 | while(SPI2.cmd.usr); 265 | encoder_int = (SPI2.data_buf[0] & 0x00FF)*256 ; 266 | 267 | SPI2.data_buf[0] = 0; 268 | SPI2.cmd.usr = 1; 269 | while(SPI2.cmd.usr); 270 | 271 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 272 | encoder_int = (SPI2.data_buf[0]& 0x00FF) + encoder_int; 273 | encoder_int = encoder_int>>4; 274 | } 275 | 276 | int insta_speed; 277 | int phase_forward; 278 | 279 | void IRAM_ATTR Motor_CTRL(void) 280 | { 281 | Motor_Position_Old = Motor_Position; 282 | Mag_Sensor_Data = encoder_int; 283 | Motor_Position = (Mag_Sensor_Data); //& 0x2FFF; 284 | 285 | if (start<2) 286 | { 287 | start++; 288 | Motor_Abs = 0; 289 | } 290 | Motor_Pos_Change = -(Motor_Position-Motor_Position_Old); 291 | insta_speed = abs(Motor_Pos_Change); 292 | 293 | if (Motor_Pos_Change>2048) 294 | off_set = Motor_Pos_Change-4096; 295 | else if (Motor_Pos_Change<-2048) 296 | off_set = Motor_Pos_Change + 4096; 297 | else 298 | off_set = Motor_Pos_Change; 299 | 300 | Motor_Abs += off_set; 301 | 302 | //Joint_Position = encoder_int; 303 | 304 | // PID Control - Just P for now 305 | Motor_Error = (Motor_Abs - Tar_Position); 306 | Motor_Joint = config_array[KP_OS] * Motor_Error; 307 | motor_output = Motor_Joint; 308 | 309 | if (abs(motor_output)<2) 310 | motor_output=0; 311 | 312 | if (insta_speed>30) 313 | insta_speed=30; 314 | if (config_array[PHASE_OFFSET_OS]>0) 315 | { 316 | phase_forward = config_array[PHASE_OFFSET_OS]-insta_speed; 317 | } 318 | else 319 | { 320 | phase_forward = config_array[PHASE_OFFSET_OS]+insta_speed; 321 | } 322 | 323 | if (motor_output > 0) // Forward or Reverse 324 | { 325 | drive = phase_forward; //phase_set; 326 | } 327 | else 328 | { 329 | drive = -phase_forward; //phase_set; 330 | motor_output = -motor_output; 331 | } 332 | 333 | if (motor_output > Maximum_PWM) 334 | motor_output = Maximum_PWM; 335 | /* 336 | if (cur_cur>2000) 337 | motor_output = motor_output/2; 338 | else if (cur_cur>3000) 339 | motor_output = 0; 340 | */ 341 | // Drive Motor at ~90deg electrically ahead of magnets 342 | Target_Phase = Motor_Position + drive + config_array[MAG_OFFSET_OS];//MAGNET_OFFSET; // MAGNET_OFFSET; 343 | 344 | //Handle wrap-around 345 | if (Target_Phase >= SIN_ARRAY_SIZE) 346 | Target_Phase = Target_Phase - SIN_ARRAY_SIZE; 347 | else if (Target_Phase < 0) 348 | Target_Phase = Target_Phase + SIN_ARRAY_SIZE; 349 | else 350 | {} 351 | } 352 | 353 | /* 354 | * 355 | */ 356 | void IRAM_ATTR Motor_Vector_Phases(int Mot_Phase, int Motor_PWM) 357 | { 358 | int U_Ang = Mot_Phase; 359 | int V_Ang = Mot_Phase + phase_120; 360 | int W_Ang = Mot_Phase - phase_120; 361 | 362 | if (V_Ang >= SIN_ARRAY_SIZE) 363 | V_Ang -=SIN_ARRAY_SIZE; 364 | 365 | if (W_Ang < 0) 366 | W_Ang +=SIN_ARRAY_SIZE; 367 | 368 | PWMUU = (SinArray[U_Ang] * Motor_PWM) >>FULL_PWM_BITS; 369 | PWMVV = (SinArray[V_Ang] * Motor_PWM) >>FULL_PWM_BITS; 370 | PWMWW = (SinArray[W_Ang] * Motor_PWM) >>FULL_PWM_BITS; 371 | } 372 | 373 | void IRAM_ATTR Three_Phases(void) 374 | { 375 | if (Motor_on==0) 376 | { 377 | PWMUU = 0; 378 | PWMVV = 0; 379 | PWMWW = 0; 380 | } 381 | if (config_array[REV_OS]==1) 382 | { 383 | MCPWM0.channel[0].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMUU; 384 | MCPWM0.channel[1].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMVV; 385 | MCPWM0.channel[2].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMWW; 386 | } 387 | else 388 | { 389 | MCPWM0.channel[0].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMVV; 390 | MCPWM0.channel[1].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMUU; 391 | MCPWM0.channel[2].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMWW; 392 | } 393 | } 394 | /* 395 | * 396 | */ 397 | void IRAM_ATTR Setup_Sin_array(void) 398 | { 399 | int step1; 400 | float tempa; 401 | float tempb; 402 | int phase; 403 | sin_scale = 1.0/((sin(60*DEG_RAD))+(sin(60*DEG_RAD))); 404 | phase_120 = SIN_ARRAY_SIZE/(MOTOR_POLE_PAIRS*3); 405 | Serial.print("phase_120 "); 406 | Serial.println(phase_120); 407 | for (step1=0; step1>8) & 0xFF; 656 | } 657 | 658 | EEPROM.put(address, eeprom_data); 659 | if (EEPROM.commit()) 660 | Serial.println("save config to eeprom"); 661 | else 662 | Serial.println("failsave config to eeprom"); 663 | } 664 | 665 | void zero_eeprom(void) 666 | { 667 | int i; 668 | int16_t config_temp; 669 | unsigned long address = 0; 670 | for (i=0; i< (CONFIG_ARRAY_SIZE); i++) 671 | { 672 | config_temp = 0; 673 | config_array[i] = config_temp; 674 | eeprom_data[i*2] = (config_temp) & 0xFF; 675 | eeprom_data[i*2 + 1]= (config_temp >>8) & 0xFF; 676 | } 677 | 678 | EEPROM.put(address, eeprom_data); 679 | if (EEPROM.commit()) 680 | Serial.println("save zero eeprom"); 681 | else 682 | Serial.println("failsave save zero eeprom"); 683 | 684 | } 685 | 686 | void eeprom_to_config(void) 687 | { 688 | unsigned long address = 0; 689 | int i; 690 | int32_t config_temp=0; 691 | Serial.println("read eeprom to config"); 692 | EEPROM.get(address, eeprom_data); 693 | for (i=0; i< (CONFIG_ARRAY_SIZE); i++) 694 | { 695 | config_temp = eeprom_data[i*2] | eeprom_data[i*2+1]<<8 ; 696 | config_array[i] = (int16_t)config_temp ; 697 | Serial.print(i); 698 | Serial.print(" "); 699 | Serial.println(config_array[i]); 700 | } 701 | Serial.println("read done"); 702 | } 703 | 704 | // CAN ///////////////////////////////////////////////////////// 705 | int data1; 706 | int data2; 707 | int data3; 708 | void IRAM_ATTR onReceive(int packetSize) { 709 | // received a packet 710 | char a; 711 | int16_t temp_data; 712 | //Serial.print("R "); 713 | 714 | int pack_id = CAN.packetId(); 715 | //Serial.print(pack_id); 716 | //Serial.print(" "); 717 | if (pack_id == config_array[ID_OS]) 718 | { 719 | if (CAN.available()) { 720 | temp_data = CAN.read()<<8; 721 | data1 = temp_data | (CAN.read()); 722 | temp_data = CAN.read()<<8; 723 | data2 = temp_data | (CAN.read()); 724 | temp_data = CAN.read()<<8; 725 | data3 = temp_data | (CAN.read()); 726 | } 727 | //Serial.print(data1); 728 | //Serial.print(" "); 729 | //Serial.println(data2); 730 | if (data1==0) 731 | { 732 | Motor_on=0; 733 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 734 | } 735 | else if(data1==1) 736 | { 737 | Motor_on=1; 738 | Tar_Position = (int) data2; 739 | Maximum_PWM = data3; 740 | fault=1; 741 | GPIO.out_w1ts= ((uint32_t)1 << DRV_EN_GATE); 742 | } 743 | else if(data1==3) 744 | { 745 | Motor_Abs=0; 746 | Motor_on=0; 747 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 748 | } 749 | 750 | } 751 | while (CAN.available()) { 752 | a=CAN.read();} 753 | } 754 | 755 | void IRAM_ATTR CAN_Init(){ 756 | CAN.setPins(CAN_RX, CAN_TX); 757 | Serial.println("CAN Sender"); 758 | 759 | // start the CAN bus at 500 kbps 760 | if (!CAN.begin(500E3)) { 761 | Serial.println("Starting CAN failed!"); 762 | //while (1); 763 | } 764 | CAN.setPins(CAN_RX, CAN_TX); 765 | 766 | CAN.onReceive(onReceive); 767 | } 768 | 769 | void IRAM_ATTR setup() { 770 | delay(2000); 771 | Serial.begin(115200); 772 | 773 | pinMode(DRV_EN_GATE, OUTPUT); 774 | pinMode (DRV_nFAULT, INPUT_PULLUP); 775 | pinMode(TEST_IO2, OUTPUT); 776 | pinMode(TEST_IO4, OUTPUT); 777 | EepromInit(); 778 | delay(500); 779 | eeprom_to_config(); 780 | pinMode(LED_PIN,OUTPUT); 781 | digitalWrite(LED_PIN,LOW); 782 | 783 | pinMode (RXD0, INPUT_PULLUP); 784 | 785 | // 786 | //https://www.toptal.com/embedded/esp32-audio-sampling 787 | //analogInit(); 788 | adcAttachPin(DRV_SO1); 789 | adcAttachPin(DRV_SO2); 790 | adcAttachPin(DRV_SO3); 791 | adcAttachPin(MOTOR_TEMP); 792 | adcAttachPin(FET_TEMP); 793 | adcAttachPin(VOLT_MON); 794 | 795 | 796 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 797 | Setup_Sin_array(); 798 | 799 | SPIInit(); 800 | setup_mcpwm(); 801 | //EepromInit(); 802 | Serial.println("setup_mcpwm"); 803 | 804 | int i; 805 | unsigned int temps; 806 | 807 | for (i=0; i<15;i++) 808 | { 809 | TMC6200_SPI_Read(i); 810 | Serial.print(i); 811 | Serial.print(" "); 812 | Serial.print(drv_spi_value[0]); 813 | Serial.print(" "); 814 | Serial.print(drv_spi_value[1]); 815 | Serial.print(" "); 816 | Serial.print(drv_spi_value[2]); 817 | Serial.print(" "); 818 | Serial.print(drv_spi_value[3]); 819 | Serial.print(" "); 820 | Serial.println(drv_spi_value[4]); 821 | } 822 | s1_nom = analogRead(DRV_SO1)+analogRead(DRV_SO1)+analogRead(DRV_SO1); 823 | s2_nom = analogRead(DRV_SO2)+analogRead(DRV_SO2)+analogRead(DRV_SO2); 824 | s3_nom = analogRead(DRV_SO3)+analogRead(DRV_SO3)+analogRead(DRV_SO3); 825 | 826 | CAN_Init(); 827 | delay(100); 828 | Serial.println("TimerInit"); 829 | delay(100); 830 | fault=1; 831 | TimerInit(); 832 | } 833 | 834 | unsigned int TMC6200_SPI_Read(unsigned int reg) 835 | { 836 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 837 | 838 | SPI2.data_buf[0] = (reg); 839 | SPI2.cmd.usr = 1; 840 | while(SPI2.cmd.usr); 841 | drv_spi_value[0] = SPI2.data_buf[0] ; 842 | 843 | SPI2.data_buf[0] = 0; 844 | SPI2.cmd.usr = 1; 845 | while(SPI2.cmd.usr); 846 | drv_spi_value[1] = SPI2.data_buf[0] ; 847 | 848 | SPI2.data_buf[0] = 0; 849 | SPI2.cmd.usr = 1; 850 | while(SPI2.cmd.usr); 851 | drv_spi_value[2] = SPI2.data_buf[0] ; 852 | 853 | SPI2.data_buf[0] = 0; 854 | SPI2.cmd.usr = 1; 855 | while(SPI2.cmd.usr); 856 | drv_spi_value[3] = SPI2.data_buf[0] ; 857 | 858 | SPI2.data_buf[0] = 0; 859 | SPI2.cmd.usr = 1; 860 | while(SPI2.cmd.usr); 861 | drv_spi_value[4] = SPI2.data_buf[0] ; 862 | 863 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 864 | //return(); 865 | } 866 | 867 | unsigned int TMC6200_SPI_Write(unsigned int reg, unsigned int data_write) 868 | { 869 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 870 | 871 | SPI2.data_buf[0] = (reg)&0x80; 872 | SPI2.cmd.usr = 1; 873 | while(SPI2.cmd.usr); 874 | drv_spi_value[0] = SPI2.data_buf[0] ; 875 | 876 | SPI2.data_buf[0] = (data_write & 0xFF000000)>>24; 877 | SPI2.cmd.usr = 1; 878 | while(SPI2.cmd.usr); 879 | drv_spi_value[1] = SPI2.data_buf[0] ; 880 | 881 | SPI2.data_buf[0] = (data_write & 0x00FF0000)>>16; 882 | SPI2.cmd.usr = 1; 883 | while(SPI2.cmd.usr); 884 | drv_spi_value[2] = SPI2.data_buf[0] ; 885 | 886 | SPI2.data_buf[0] = (data_write & 0x0000FF00)>>8; 887 | SPI2.cmd.usr = 1; 888 | while(SPI2.cmd.usr); 889 | drv_spi_value[3] = SPI2.data_buf[0] ; 890 | 891 | SPI2.data_buf[0] = (data_write & 0x000000FF); 892 | SPI2.cmd.usr = 1; 893 | while(SPI2.cmd.usr); 894 | drv_spi_value[4] = SPI2.data_buf[0] ; 895 | 896 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 897 | //return(); 898 | } 899 | 900 | int IRAM_ATTR local_adc1_read(int channel) { 901 | uint16_t adc_value; 902 | SENS.sar_meas_start1.sar1_en_pad = (1 << channel); // only one channel is selected 903 | while (SENS.sar_slave_addr1.meas_status != 0); 904 | SENS.sar_meas_start1.meas1_start_sar = 0; 905 | SENS.sar_meas_start1.meas1_start_sar = 1; 906 | while (SENS.sar_meas_start1.meas1_done_sar == 0); 907 | adc_value = SENS.sar_meas_start1.meas1_data_sar; 908 | return adc_value; 909 | } 910 | int voltage_value; 911 | 912 | 913 | #define voltage_scale (1.0/18.16) 914 | #define ANGLE_OFFSET -140 915 | 916 | void IRAM_ATTR loop() { 917 | int i; 918 | static int adc_cnt; 919 | unsigned int temp; 920 | char ser_read; 921 | count++; 922 | if (count>99) 923 | count=0; 924 | GPIO.out_w1ts = ((uint32_t)1 << TEST_IO4); //TESTPINB 925 | /* 926 | s1 = (analogRead(DRV_SO1)+analogRead(DRV_SO1)+analogRead(DRV_SO1))- s1_nom; 927 | s2 = (analogRead(DRV_SO2)+analogRead(DRV_SO2)+analogRead(DRV_SO2))- s2_nom; 928 | s3 = (analogRead(DRV_SO3)+analogRead(DRV_SO3)+analogRead(DRV_SO3))- s3_nom; 929 | 930 | cur_cur = (abs(s1) + abs(s2) + abs(s3)); 931 | tt = analogRead(FET_TEMP); 932 | tm = analogRead(VOLT_MON); 933 | 934 | Serial.print (cur_cur); 935 | 936 | Serial.print(" tt "); 937 | Serial.print (tt); 938 | Serial.print(" tm "); 939 | Serial.print (tm); 940 | //Serial.print(" "); 941 | 942 | 943 | Serial.print(" T "); 944 | Serial.println(Tar_Position); 945 | //Serial.print(" ABS "); 946 | //Serial.print(Motor_Abs); 947 | //Serial.print(" max "); 948 | //Serial.println(Maximum_PWM); 949 | */ 950 | //Serial.print(" max "); 951 | Serial.print(Maximum_PWM); 952 | Serial.print(" T "); 953 | Serial.println(Tar_Position); 954 | 955 | GPIO.out_w1tc = ((uint32_t)1 << TEST_IO4); //TESTPINB 956 | 957 | if ( Serial.available() ) 958 | { 959 | if (Serial.available()) 960 | { 961 | ser_read = Serial.read(); 962 | Serial.println(ser_read); 963 | } 964 | 965 | if (ser_read=='l') 966 | { 967 | Serial.println( config_array[MAG_OFFSET_OS]); 968 | config_array[MAG_OFFSET_OS]++; 969 | } 970 | else if(ser_read=='k') 971 | { 972 | Serial.println( config_array[MAG_OFFSET_OS]); 973 | config_array[MAG_OFFSET_OS]--; 974 | } 975 | else if (ser_read=='n') 976 | { 977 | Serial.println(config_array[PHASE_OFFSET_OS]); 978 | config_array[PHASE_OFFSET_OS]++; 979 | } 980 | else if(ser_read=='m') 981 | { 982 | Serial.println( config_array[PHASE_OFFSET_OS]); 983 | config_array[PHASE_OFFSET_OS]--; 984 | } 985 | else if (ser_read=='e') 986 | { 987 | edit_config(); 988 | } 989 | 990 | else if(ser_read=='q') 991 | { 992 | Motor_on=0; 993 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 994 | } 995 | /* 996 | else if(ser_read=='w') 997 | { 998 | fault=0; 999 | delay(10); 1000 | for (i=0; i<15;i++) 1001 | { 1002 | //temp=DRV8305_SPI_Read(i); 1003 | Serial.print(i); 1004 | Serial.print(" "); 1005 | Serial.println(temp, HEX); 1006 | } 1007 | fault=1; 1008 | Motor_on=1; 1009 | GPIO.out_w1ts= ((uint32_t)1 << DRV_EN_GATE); 1010 | } 1011 | */ 1012 | else if (ser_read=='f') 1013 | { 1014 | fault=0; 1015 | delay(10); 1016 | for (i=0; i<15;i++) 1017 | { 1018 | TMC6200_SPI_Read(i); 1019 | Serial.print(i); 1020 | Serial.print(" "); 1021 | Serial.print(drv_spi_value[0]); 1022 | Serial.print(" "); 1023 | Serial.print(drv_spi_value[1]); 1024 | Serial.print(" "); 1025 | Serial.print(drv_spi_value[2]); 1026 | Serial.print(" "); 1027 | Serial.print(drv_spi_value[3]); 1028 | Serial.print(" "); 1029 | Serial.println(drv_spi_value[4]); 1030 | } 1031 | //Serial4_DRV_Write(7, 0x296); 1032 | fault=1; 1033 | } 1034 | 1035 | } 1036 | 1037 | } 1038 | -------------------------------------------------------------------------------- /BLDC_ESP32_b.ino: -------------------------------------------------------------------------------- 1 | //include 2 | 3 | 4 | #include 5 | #include "driver/mcpwm.h" 6 | #include "soc/mcpwm_reg.h" 7 | #include "soc/mcpwm_struct.h" 8 | #include "driver/periph_ctrl.h" 9 | #include "driver/spi_master.h" 10 | #include "soc/gpio_sig_map.h" 11 | #include "soc/spi_reg.h" 12 | #include "soc/dport_reg.h" 13 | #include "soc/spi_struct.h" 14 | #include "soc/sens_reg.h" 15 | #include "soc/sens_struct.h" 16 | #include "esp_adc_cal.h" 17 | #include 18 | 19 | #include "BluetoothSerial.h" 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #define WIFI_CHANNEL (1) 27 | #define LED_PIN (2) 28 | 29 | //////////////////////////// wifi 30 | 31 | static uint8_t broadcast_mac[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; 32 | 33 | typedef struct __attribute__((packed)) esp_now_msg_t 34 | { 35 | uint32_t address; 36 | uint32_t counter; 37 | // Can put lots of things here... 38 | } esp_now_msg_t; 39 | 40 | 41 | #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) 42 | #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it 43 | #endif 44 | 45 | BluetoothSerial SerialBT; 46 | 47 | //#include "wiring_private.h" // pinPeripheral() function 48 | 49 | // I2c OLED /////////////////////////////////////////////////////////// 50 | #include "ssd1306.h" // library by Alexey Dynda 51 | char display_str[] = "1234567890"; 52 | #include "EEPROM.h" 53 | 54 | int addr = 0; 55 | #define EEPROM_SIZE 64 56 | 57 | //GPIO.out_w1tc and GPIO.out_w1ts 58 | 59 | int count; 60 | 61 | static const int spiClk = 2000000; // 1 MHz 62 | SPIClass * hspi = NULL; 63 | 64 | 65 | hw_timer_t * timer = NULL; 66 | portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; 67 | int hi_byte=0; 68 | int lo_byte=0; 69 | int encoder_int =0; 70 | int encoder_ext = 0; 71 | int drv_spi_value = 0; 72 | 73 | /////////////////////////////////////////////////////////////////////////////////////// 74 | int man_offset=180; 75 | #define SIN_ARRAY_SIZE_BITS 12 76 | #define SIN_ARRAY_SIZE (1<begin(); 188 | pinMode(INT_ENC_nCS, OUTPUT); 189 | pinMode(EXT_ENC_nCS, OUTPUT); 190 | pinMode(DRV_nSCS, OUTPUT); 191 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 192 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 193 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 194 | hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE1)); //AS5047 195 | //hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE3)); //MA702 196 | SPI2.mosi_dlen.usr_mosi_dbitlen = (1 * 16) - 1; 197 | SPI2.miso_dlen.usr_miso_dbitlen = (1 * 16) - 1; 198 | } 199 | 200 | int even=0; 201 | void IRAM_ATTR SPI_Read_Encoder(){ 202 | GPIO.out_w1tc = ((uint32_t)1 << INT_ENC_nCS); 203 | SPI2.data_buf[0] = 0xFFFF; //AS5147 204 | //SPI2.data_buf[0] = 0x0000; //MA702 205 | SPI2.cmd.usr = 1; 206 | while(SPI2.cmd.usr); 207 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 208 | encoder_int = SPI2.data_buf[0] & 0xFFFF; 209 | if(!SPI2.ctrl.rd_bit_order){ 210 | encoder_int = (encoder_int >> 8) | (encoder_int << 8); 211 | } 212 | encoder_int = encoder_int & 0x3FFF ; 213 | 214 | if (even){ 215 | GPIO.out_w1tc = ((uint32_t)1 << EXT_ENC_nCS); 216 | SPI2.data_buf[0] = 0xFFFF; //AS5147 217 | //SPI2.data_buf[0] = 0x0000; //MA702 218 | SPI2.cmd.usr = 1; 219 | while(SPI2.cmd.usr); 220 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 221 | encoder_ext = SPI2.data_buf[0] & 0xFFFF; 222 | if(!SPI2.ctrl.rd_bit_order){ 223 | encoder_ext = (encoder_ext >> 8) | (encoder_ext << 8); 224 | } 225 | encoder_ext = encoder_ext & 0x3FFF ; 226 | even=0; 227 | } 228 | else 229 | { 230 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 231 | SPI2.data_buf[0] = 0xFFFF; //AS5147 232 | //SPI2.data_buf[0] = 0x0000; //MA702 233 | SPI2.cmd.usr = 1; 234 | while(SPI2.cmd.usr); 235 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 236 | drv_spi_value = SPI2.data_buf[0] & 0xFFFF; 237 | if(!SPI2.ctrl.rd_bit_order){ 238 | drv_spi_value = (drv_spi_value >> 8) | (drv_spi_value << 8); 239 | } 240 | //drv_spi_value 241 | even=1; 242 | } 243 | } 244 | 245 | 246 | 247 | void IRAM_ATTR Motor_CTRL(void) 248 | { 249 | Motor_Position_Old = Motor_Position; 250 | Mag_Sensor_Data = encoder_int; 251 | Motor_Position = (Mag_Sensor_Data>>2); //& 0x2FFF; 252 | 253 | if (start<2) 254 | { 255 | start++; 256 | Motor_Abs = 0; 257 | } 258 | Motor_Pos_Change = (Motor_Position-Motor_Position_Old); 259 | 260 | if (Motor_Pos_Change>2048) 261 | off_set = Motor_Pos_Change-4096; 262 | else if (Motor_Pos_Change<-2048) 263 | off_set = Motor_Pos_Change + 4096; 264 | else 265 | off_set = Motor_Pos_Change; 266 | 267 | Motor_Abs += off_set; 268 | 269 | Joint_Position = Motor_Abs; 270 | 271 | // PID Control - Just P for now 272 | Motor_Error = (Joint_Position - Tar_Position); 273 | Motor_Joint = -Kp * Motor_Error; 274 | /* 275 | if (forward) 276 | Motor_Joint = 250; 277 | else 278 | Motor_Joint = -250; 279 | */ 280 | 281 | if (Motor_Joint > 0) // Forward or Reverse 282 | { 283 | drive = PHASE_OFFSET; //phase_set; 284 | } 285 | else 286 | { 287 | drive = -PHASE_OFFSET; //phase_set; 288 | Motor_Joint = -Motor_Joint; 289 | } 290 | // Limit Voltage/PWM to motor 291 | if (Motor_Joint > MAX_PWM) 292 | Motor_Joint = MAX_PWM; 293 | 294 | // Drive Motor at ~90deg electrically ahead of magnets 295 | Target_Phase = Motor_Position + drive + man_offset;//MAGNET_OFFSET; // MAGNET_OFFSET; 296 | 297 | //Handle wrap-around 298 | if (Target_Phase >= SIN_ARRAY_SIZE) 299 | Target_Phase = Target_Phase - SIN_ARRAY_SIZE; 300 | else if (Target_Phase < 0) 301 | Target_Phase = Target_Phase + SIN_ARRAY_SIZE; 302 | else 303 | {} 304 | } 305 | 306 | /* 307 | * 308 | */ 309 | void IRAM_ATTR Motor_Vector_Phases(int Mot_Phase, int Motor_PWM) 310 | { 311 | int U_Ang = Mot_Phase; 312 | int V_Ang = Mot_Phase + phase_120; 313 | int W_Ang = Mot_Phase - phase_120; 314 | 315 | if (V_Ang >= SIN_ARRAY_SIZE) 316 | V_Ang -=SIN_ARRAY_SIZE; 317 | 318 | if (W_Ang < 0) 319 | W_Ang +=SIN_ARRAY_SIZE; 320 | 321 | PWMUU = (SinArray[U_Ang] * Motor_PWM) >>FULL_PWM_BITS; 322 | PWMVV = (SinArray[V_Ang] * Motor_PWM) >>FULL_PWM_BITS; 323 | PWMWW = (SinArray[W_Ang] * Motor_PWM) >>FULL_PWM_BITS; 324 | } 325 | 326 | void IRAM_ATTR Three_Phases(void) 327 | { 328 | if (Motor_on==0) 329 | { 330 | PWMUU = 0; 331 | PWMVV = 0; 332 | PWMWW = 0; 333 | } 334 | MCPWM0.channel[0].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMUU; 335 | MCPWM0.channel[1].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMVV; 336 | MCPWM0.channel[2].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMWW; 337 | } 338 | /* 339 | * 340 | */ 341 | void IRAM_ATTR Setup_Sin_array(void) 342 | { 343 | int step1; 344 | float tempa; 345 | float tempb; 346 | int phase; 347 | sin_scale = 1.0/((sin(60*DEG_RAD))+(sin(60*DEG_RAD))); 348 | phase_120 = SIN_ARRAY_SIZE/(MOTOR_POLE_PAIRS*3); 349 | for (step1=0; step1> 8) | (drv_spi_value << 8); 716 | } 717 | return(drv_spi_value & 0xFFFF); 718 | } 719 | 720 | unsigned int DRV8305_SPI_Write(unsigned int reg, unsigned int data_write) 721 | { 722 | unsigned int drv_spi_value; 723 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 724 | SPI2.data_buf[0] = (reg<<3) + ((data_write & 0x1F00)>>8) + ((data_write & 0x00FF)<<8); 725 | SPI2.cmd.usr = 1; 726 | while(SPI2.cmd.usr); 727 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 728 | drv_spi_value = SPI2.data_buf[0] & 0xFFFF; 729 | if(!SPI2.ctrl.rd_bit_order){ 730 | drv_spi_value = (drv_spi_value >> 8) | (drv_spi_value << 8); 731 | } 732 | return(drv_spi_value); 733 | } 734 | 735 | int IRAM_ATTR local_adc1_read(int channel) { 736 | uint16_t adc_value; 737 | SENS.sar_meas_start1.sar1_en_pad = (1 << channel); // only one channel is selected 738 | while (SENS.sar_slave_addr1.meas_status != 0); 739 | SENS.sar_meas_start1.meas1_start_sar = 0; 740 | SENS.sar_meas_start1.meas1_start_sar = 1; 741 | while (SENS.sar_meas_start1.meas1_done_sar == 0); 742 | adc_value = SENS.sar_meas_start1.meas1_data_sar; 743 | return adc_value; 744 | } 745 | int voltage_value; 746 | 747 | static void handle_error(esp_err_t err) 748 | { 749 | switch (err) 750 | { 751 | case ESP_ERR_ESPNOW_NOT_INIT: 752 | Serial.println("Not init"); 753 | break; 754 | 755 | case ESP_ERR_ESPNOW_ARG: 756 | Serial.println("Argument invalid"); 757 | break; 758 | 759 | case ESP_ERR_ESPNOW_INTERNAL: 760 | Serial.println("Internal error"); 761 | break; 762 | 763 | case ESP_ERR_ESPNOW_NO_MEM: 764 | Serial.println("Out of memory"); 765 | break; 766 | 767 | case ESP_ERR_ESPNOW_NOT_FOUND: 768 | Serial.println("Peer is not found"); 769 | break; 770 | 771 | case ESP_ERR_ESPNOW_IF: 772 | Serial.println("Current WiFi interface doesn't match that of peer"); 773 | break; 774 | 775 | default: 776 | break; 777 | } 778 | } 779 | 780 | static void msg_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len) 781 | { 782 | //Serial.println("RX_MESSAGE"); 783 | if (len == sizeof(esp_now_msg_t)) 784 | { 785 | esp_now_msg_t msg; 786 | memcpy(&msg, data, len); 787 | d_counter = msg.counter; 788 | //Serial.print("Counter: "); 789 | //Serial.println(msg.counter); 790 | //digitalWrite(LED_PIN, !digitalRead(LED_PIN)); 791 | } 792 | } 793 | 794 | static void msg_send_cb(const uint8_t* mac, esp_now_send_status_t sendStatus) 795 | { 796 | 797 | switch (sendStatus) 798 | { 799 | case ESP_NOW_SEND_SUCCESS: 800 | Serial.println("S"); 801 | break; 802 | 803 | case ESP_NOW_SEND_FAIL: 804 | Serial.println("F"); 805 | break; 806 | 807 | default: 808 | break; 809 | } 810 | } 811 | 812 | static void send_msg(esp_now_msg_t * msg) 813 | { 814 | // Pack 815 | uint16_t packet_size = sizeof(esp_now_msg_t); 816 | uint8_t msg_data[packet_size]; 817 | memcpy(&msg_data[0], msg, sizeof(esp_now_msg_t)); 818 | 819 | esp_err_t status = esp_now_send(broadcast_mac, msg_data, packet_size); 820 | if (ESP_OK != status) 821 | { 822 | Serial.println("Error sending message"); 823 | handle_error(status); 824 | } 825 | } 826 | static void send_esp_now(void) 827 | { 828 | static uint32_t counter = 0; 829 | esp_now_msg_t msg; 830 | msg.address = 0; 831 | msg.counter = ++counter; 832 | send_msg(&msg); 833 | } 834 | static void network_setup(void) 835 | { 836 | //esp_wifi_internal_set_fix_rate(10); 837 | //Puts ESP in STATION MODE 838 | WiFi.mode(WIFI_STA); 839 | WiFi.disconnect(); 840 | 841 | if (esp_now_init() != 0) 842 | { 843 | Serial.println("esp_now_init Fail"); 844 | return; 845 | } 846 | 847 | esp_now_peer_info_t peer_info; 848 | peer_info.channel = WIFI_CHANNEL; 849 | memcpy(peer_info.peer_addr, broadcast_mac, 6); 850 | peer_info.ifidx = ESP_IF_WIFI_STA; 851 | peer_info.encrypt = false; 852 | esp_err_t status = esp_now_add_peer(&peer_info); 853 | if (ESP_OK != status) 854 | { 855 | Serial.println("Could not add peer"); 856 | handle_error(status); 857 | } 858 | 859 | // Set up callback 860 | status = esp_now_register_recv_cb(msg_recv_cb); 861 | if (ESP_OK != status) 862 | { 863 | Serial.println("Could not register callback"); 864 | handle_error(status); 865 | } 866 | 867 | status = esp_now_register_send_cb(msg_send_cb); 868 | if (ESP_OK != status) 869 | { 870 | Serial.println("Could not register send callback"); 871 | handle_error(status); 872 | } 873 | } 874 | 875 | #define voltage_scale (1.0/18.16) 876 | void IRAM_ATTR loop() { 877 | int i; 878 | unsigned int temp; 879 | char ser_read; 880 | count++; 881 | if (count>99) 882 | count=0; 883 | 884 | 885 | voltage_value = analogRead(VOLT_MON); 886 | voltage = (float)voltage_value * voltage_scale; 887 | textDemo(); 888 | //delay(10); 889 | //CAN_Send(); 890 | send_esp_now(); 891 | 892 | //delay(50); 893 | 894 | GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 895 | /* 896 | //analogRead(VOLT_MON); 897 | if (SerialBT.hasClient()) 898 | Serial.print ("BT "); 899 | else 900 | Serial.print ("OFF "); 901 | 902 | Serial.print (man_offset); 903 | Serial.print (" "); 904 | Serial.println (analogRead(VOLT_MON)); 905 | */ 906 | //Serial.println (local_adc1_read(DRV_SO1)); 907 | /* 908 | Serial.print (" "); 909 | Serial.print (analogRead(local_adc1_read(DRV_SO2))); 910 | Serial.print (" "); 911 | Serial.print (analogRead(local_adc1_read(DRV_SO3))); 912 | Serial.print (" "); 913 | Serial.print (analogRead(local_adc1_read(VOLT_MON))); 914 | Serial.print (" "); 915 | Serial.print (analogRead(local_adc1_read(MOTOR_TEMP))); 916 | Serial.print (" "); 917 | Serial.println (analogRead(local_adc1_read(VOLT_MON))); 918 | */ 919 | 920 | GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 921 | //GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 922 | //analogRead(DRV_SO2); 923 | //GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 924 | // GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 925 | //analogRead(DRV_SO3); 926 | //GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 927 | //SerialBT.print("@ "); 928 | /* 929 | if (Serial.available()) 930 | { 931 | ser_read = Serial.read(); 932 | } 933 | */ 934 | 935 | if (SerialBT.available()) 936 | { 937 | //ser_read = SerialBT.read(); 938 | ser_read = SerialBT.read(); 939 | SerialBT.println(ser_read); 940 | if (ser_read=='l') 941 | { 942 | man_offset++; 943 | } 944 | else if(ser_read=='q') 945 | { 946 | Motor_on=0; 947 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 948 | } 949 | else if(ser_read=='w') 950 | { 951 | Motor_on=1; 952 | GPIO.out_w1ts= ((uint32_t)1 << DRV_EN_GATE); 953 | } 954 | else if(ser_read=='k') 955 | { 956 | man_offset--; 957 | } 958 | else if (ser_read=='b') 959 | { 960 | Motor_Abs=0; 961 | } 962 | 963 | else if (ser_read=='a') 964 | { 965 | Tar_Position=-8000; 966 | //forward=0; 967 | } 968 | else if (ser_read=='s') 969 | { 970 | Tar_Position=0; 971 | //forward=1; 972 | } 973 | else if (ser_read=='d') 974 | { 975 | Tar_Position=8000; 976 | //forward=1; 977 | } 978 | else if (ser_read=='f') 979 | { 980 | fault=0; 981 | delay(10); 982 | for (i=0; i<15;i++) 983 | { 984 | temp=DRV8305_SPI_Read(i); 985 | Serial.print(i); 986 | Serial.print(" "); 987 | Serial.println(temp, HEX); 988 | } 989 | //Serial4_DRV_Write(7, 0x296); 990 | fault=1; 991 | } 992 | 993 | } 994 | 995 | } 996 | -------------------------------------------------------------------------------- /BLDC_ESP32_now.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "driver/mcpwm.h" 4 | #include "soc/mcpwm_reg.h" 5 | #include "soc/mcpwm_struct.h" 6 | #include "driver/periph_ctrl.h" 7 | #include "driver/spi_master.h" 8 | #include "soc/gpio_sig_map.h" 9 | #include "soc/spi_reg.h" 10 | #include "soc/dport_reg.h" 11 | #include "soc/spi_struct.h" 12 | #include "soc/sens_reg.h" 13 | #include "soc/sens_struct.h" 14 | #include "esp_adc_cal.h" 15 | #include 16 | 17 | #include "BluetoothSerial.h" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #define WIFI_CHANNEL (1) 25 | #define LED_PIN (2) 26 | 27 | //////////////////////////// wifi 28 | 29 | static uint8_t broadcast_mac[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; 30 | 31 | typedef struct __attribute__((packed)) esp_now_msg_t 32 | { 33 | uint32_t address; 34 | int32_t now_data; 35 | // Can put lots of things here... 36 | } esp_now_msg_t; 37 | 38 | int32_t other_motor_pos; 39 | 40 | #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) 41 | #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it 42 | #endif 43 | 44 | BluetoothSerial SerialBT; 45 | 46 | //#include "wiring_private.h" // pinPeripheral() function 47 | 48 | // I2c OLED /////////////////////////////////////////////////////////// 49 | #include "ssd1306.h" // library by Alexey Dynda 50 | char display_str[] = "1234567890"; 51 | #include "EEPROM.h" 52 | 53 | int addr = 0; 54 | #define EEPROM_SIZE 64 55 | 56 | //GPIO.out_w1tc and GPIO.out_w1ts 57 | 58 | int count; 59 | 60 | static const int spiClk = 2000000; // 1 MHz 61 | SPIClass * hspi = NULL; 62 | 63 | 64 | hw_timer_t * timer = NULL; 65 | portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; 66 | int hi_byte=0; 67 | int lo_byte=0; 68 | int encoder_int =0; 69 | int encoder_ext = 0; 70 | int drv_spi_value = 0; 71 | 72 | /////////////////////////////////////////////////////////////////////////////////////// 73 | int man_offset=180; 74 | #define SIN_ARRAY_SIZE_BITS 12 75 | #define SIN_ARRAY_SIZE (1<begin(); 187 | pinMode(INT_ENC_nCS, OUTPUT); 188 | pinMode(EXT_ENC_nCS, OUTPUT); 189 | pinMode(DRV_nSCS, OUTPUT); 190 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 191 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 192 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 193 | hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE1)); //AS5047 194 | //hspi->beginTransaction(SPISettings(spiClk, MSBFIRST, SPI_MODE3)); //MA702 195 | SPI2.mosi_dlen.usr_mosi_dbitlen = (1 * 16) - 1; 196 | SPI2.miso_dlen.usr_miso_dbitlen = (1 * 16) - 1; 197 | } 198 | 199 | int even=0; 200 | void IRAM_ATTR SPI_Read_Encoder(){ 201 | GPIO.out_w1tc = ((uint32_t)1 << INT_ENC_nCS); 202 | SPI2.data_buf[0] = 0xFFFF; //AS5147 203 | //SPI2.data_buf[0] = 0x0000; //MA702 204 | SPI2.cmd.usr = 1; 205 | while(SPI2.cmd.usr); 206 | GPIO.out_w1ts = ((uint32_t)1 << INT_ENC_nCS); 207 | encoder_int = SPI2.data_buf[0] & 0xFFFF; 208 | if(!SPI2.ctrl.rd_bit_order){ 209 | encoder_int = (encoder_int >> 8) | (encoder_int << 8); 210 | } 211 | encoder_int = encoder_int & 0x3FFF ; 212 | 213 | if (even){ 214 | GPIO.out_w1tc = ((uint32_t)1 << EXT_ENC_nCS); 215 | SPI2.data_buf[0] = 0xFFFF; //AS5147 216 | //SPI2.data_buf[0] = 0x0000; //MA702 217 | SPI2.cmd.usr = 1; 218 | while(SPI2.cmd.usr); 219 | GPIO.out_w1ts = ((uint32_t)1 << EXT_ENC_nCS); 220 | encoder_ext = SPI2.data_buf[0] & 0xFFFF; 221 | if(!SPI2.ctrl.rd_bit_order){ 222 | encoder_ext = (encoder_ext >> 8) | (encoder_ext << 8); 223 | } 224 | encoder_ext = encoder_ext & 0x3FFF ; 225 | even=0; 226 | } 227 | else 228 | { 229 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 230 | SPI2.data_buf[0] = 0xFFFF; //AS5147 231 | //SPI2.data_buf[0] = 0x0000; //MA702 232 | SPI2.cmd.usr = 1; 233 | while(SPI2.cmd.usr); 234 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 235 | drv_spi_value = SPI2.data_buf[0] & 0xFFFF; 236 | if(!SPI2.ctrl.rd_bit_order){ 237 | drv_spi_value = (drv_spi_value >> 8) | (drv_spi_value << 8); 238 | } 239 | //drv_spi_value 240 | even=1; 241 | } 242 | } 243 | 244 | 245 | 246 | void IRAM_ATTR Motor_CTRL(void) 247 | { 248 | Motor_Position_Old = Motor_Position; 249 | Mag_Sensor_Data = encoder_int; 250 | Motor_Position = (Mag_Sensor_Data>>2); //& 0x2FFF; 251 | 252 | if (start<2) 253 | { 254 | start++; 255 | Motor_Abs = 0; 256 | } 257 | Motor_Pos_Change = -(Motor_Position-Motor_Position_Old); 258 | 259 | if (Motor_Pos_Change>2048) 260 | off_set = Motor_Pos_Change-4096; 261 | else if (Motor_Pos_Change<-2048) 262 | off_set = Motor_Pos_Change + 4096; 263 | else 264 | off_set = Motor_Pos_Change; 265 | 266 | Motor_Abs += off_set; 267 | 268 | Joint_Position = Motor_Abs; 269 | 270 | // PID Control - Just P for now 271 | //Motor_Error = (Joint_Position - Tar_Position); 272 | Motor_Error = (Joint_Position - other_motor_pos)/2; 273 | Motor_Joint = -Kp * Motor_Error; 274 | /* 275 | if (forward) 276 | Motor_Joint = 250; 277 | else 278 | Motor_Joint = -250; 279 | */ 280 | 281 | if (Motor_Joint > 0) // Forward or Reverse 282 | { 283 | drive = PHASE_OFFSET; //phase_set; 284 | } 285 | else 286 | { 287 | drive = -PHASE_OFFSET; //phase_set; 288 | Motor_Joint = -Motor_Joint; 289 | } 290 | // Limit Voltage/PWM to motor 291 | if (Motor_Joint > MAX_PWM) 292 | Motor_Joint = MAX_PWM; 293 | 294 | // Drive Motor at ~90deg electrically ahead of magnets 295 | Target_Phase = Motor_Position + drive + man_offset;//MAGNET_OFFSET; // MAGNET_OFFSET; 296 | 297 | //Handle wrap-around 298 | if (Target_Phase >= SIN_ARRAY_SIZE) 299 | Target_Phase = Target_Phase - SIN_ARRAY_SIZE; 300 | else if (Target_Phase < 0) 301 | Target_Phase = Target_Phase + SIN_ARRAY_SIZE; 302 | else 303 | {} 304 | } 305 | 306 | /* 307 | * 308 | */ 309 | void IRAM_ATTR Motor_Vector_Phases(int Mot_Phase, int Motor_PWM) 310 | { 311 | int U_Ang = Mot_Phase; 312 | int V_Ang = Mot_Phase + phase_120; 313 | int W_Ang = Mot_Phase - phase_120; 314 | 315 | if (V_Ang >= SIN_ARRAY_SIZE) 316 | V_Ang -=SIN_ARRAY_SIZE; 317 | 318 | if (W_Ang < 0) 319 | W_Ang +=SIN_ARRAY_SIZE; 320 | 321 | PWMUU = (SinArray[U_Ang] * Motor_PWM) >>FULL_PWM_BITS; 322 | PWMVV = (SinArray[V_Ang] * Motor_PWM) >>FULL_PWM_BITS; 323 | PWMWW = (SinArray[W_Ang] * Motor_PWM) >>FULL_PWM_BITS; 324 | } 325 | 326 | void IRAM_ATTR Three_Phases(void) 327 | { 328 | if (Motor_on==0) 329 | { 330 | PWMUU = 0; 331 | PWMVV = 0; 332 | PWMWW = 0; 333 | } 334 | MCPWM0.channel[0].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMUU; 335 | MCPWM0.channel[1].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMVV; 336 | MCPWM0.channel[2].cmpr_value[MCPWM_OPR_A].cmpr_val = PWMWW; 337 | } 338 | /* 339 | * 340 | */ 341 | void IRAM_ATTR Setup_Sin_array(void) 342 | { 343 | int step1; 344 | float tempa; 345 | float tempb; 346 | int phase; 347 | sin_scale = 1.0/((sin(60*DEG_RAD))+(sin(60*DEG_RAD))); 348 | phase_120 = SIN_ARRAY_SIZE/(MOTOR_POLE_PAIRS*3); 349 | for (step1=0; step1> 8) | (drv_spi_value << 8); 693 | } 694 | return(drv_spi_value & 0xFFFF); 695 | } 696 | 697 | unsigned int DRV8305_SPI_Write(unsigned int reg, unsigned int data_write) 698 | { 699 | unsigned int drv_spi_value; 700 | GPIO.out_w1tc = ((uint32_t)1 << DRV_nSCS); 701 | SPI2.data_buf[0] = (reg<<3) + ((data_write & 0x1F00)>>8) + ((data_write & 0x00FF)<<8); 702 | SPI2.cmd.usr = 1; 703 | while(SPI2.cmd.usr); 704 | GPIO.out_w1ts = ((uint32_t)1 << DRV_nSCS); 705 | drv_spi_value = SPI2.data_buf[0] & 0xFFFF; 706 | if(!SPI2.ctrl.rd_bit_order){ 707 | drv_spi_value = (drv_spi_value >> 8) | (drv_spi_value << 8); 708 | } 709 | return(drv_spi_value); 710 | } 711 | 712 | int IRAM_ATTR local_adc1_read(int channel) { 713 | uint16_t adc_value; 714 | SENS.sar_meas_start1.sar1_en_pad = (1 << channel); // only one channel is selected 715 | while (SENS.sar_slave_addr1.meas_status != 0); 716 | SENS.sar_meas_start1.meas1_start_sar = 0; 717 | SENS.sar_meas_start1.meas1_start_sar = 1; 718 | while (SENS.sar_meas_start1.meas1_done_sar == 0); 719 | adc_value = SENS.sar_meas_start1.meas1_data_sar; 720 | return adc_value; 721 | } 722 | int voltage_value; 723 | 724 | static void handle_error(esp_err_t err) 725 | { 726 | switch (err) 727 | { 728 | case ESP_ERR_ESPNOW_NOT_INIT: 729 | Serial.println("Not init"); 730 | break; 731 | 732 | case ESP_ERR_ESPNOW_ARG: 733 | Serial.println("Argument invalid"); 734 | break; 735 | 736 | case ESP_ERR_ESPNOW_INTERNAL: 737 | Serial.println("Internal error"); 738 | break; 739 | 740 | case ESP_ERR_ESPNOW_NO_MEM: 741 | Serial.println("Out of memory"); 742 | break; 743 | 744 | case ESP_ERR_ESPNOW_NOT_FOUND: 745 | Serial.println("Peer is not found"); 746 | break; 747 | 748 | case ESP_ERR_ESPNOW_IF: 749 | Serial.println("Current WiFi interface doesn't match that of peer"); 750 | break; 751 | 752 | default: 753 | break; 754 | } 755 | } 756 | 757 | static void msg_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len) 758 | { 759 | //Serial.println("RX_MESSAGE"); 760 | if (len == sizeof(esp_now_msg_t)) 761 | { 762 | esp_now_msg_t msg; 763 | memcpy(&msg, data, len); 764 | other_motor_pos = msg.now_data; 765 | //Serial.print("Counter: "); 766 | //Serial.println(msg.counter); 767 | //digitalWrite(LED_PIN, !digitalRead(LED_PIN)); 768 | } 769 | } 770 | 771 | static void msg_send_cb(const uint8_t* mac, esp_now_send_status_t sendStatus) 772 | { 773 | 774 | switch (sendStatus) 775 | { 776 | case ESP_NOW_SEND_SUCCESS: 777 | Serial.println("S"); 778 | break; 779 | 780 | case ESP_NOW_SEND_FAIL: 781 | Serial.println("F"); 782 | break; 783 | 784 | default: 785 | break; 786 | } 787 | } 788 | 789 | static void send_msg(esp_now_msg_t * msg) 790 | { 791 | // Pack 792 | uint16_t packet_size = sizeof(esp_now_msg_t); 793 | uint8_t msg_data[packet_size]; 794 | memcpy(&msg_data[0], msg, sizeof(esp_now_msg_t)); 795 | 796 | esp_err_t status = esp_now_send(broadcast_mac, msg_data, packet_size); 797 | if (ESP_OK != status) 798 | { 799 | Serial.println("Error sending message"); 800 | handle_error(status); 801 | } 802 | } 803 | static void send_esp_now(void) 804 | { 805 | static uint32_t counter = 0; 806 | esp_now_msg_t msg; 807 | msg.address = 0; 808 | msg.now_data = Motor_Abs; 809 | send_msg(&msg); 810 | } 811 | static void network_setup(void) 812 | { 813 | //esp_wifi_internal_set_fix_rate(10); 814 | //Puts ESP in STATION MODE 815 | WiFi.mode(WIFI_STA); 816 | WiFi.disconnect(); 817 | 818 | if (esp_now_init() != 0) 819 | { 820 | Serial.println("esp_now_init Fail"); 821 | return; 822 | } 823 | 824 | esp_now_peer_info_t peer_info; 825 | peer_info.channel = WIFI_CHANNEL; 826 | memcpy(peer_info.peer_addr, broadcast_mac, 6); 827 | peer_info.ifidx = ESP_IF_WIFI_STA; 828 | peer_info.encrypt = false; 829 | esp_err_t status = esp_now_add_peer(&peer_info); 830 | if (ESP_OK != status) 831 | { 832 | Serial.println("Could not add peer"); 833 | handle_error(status); 834 | } 835 | 836 | // Set up callback 837 | status = esp_now_register_recv_cb(msg_recv_cb); 838 | if (ESP_OK != status) 839 | { 840 | Serial.println("Could not register callback"); 841 | handle_error(status); 842 | } 843 | 844 | status = esp_now_register_send_cb(msg_send_cb); 845 | if (ESP_OK != status) 846 | { 847 | Serial.println("Could not register send callback"); 848 | handle_error(status); 849 | } 850 | } 851 | 852 | #define voltage_scale (1.0/18.16) 853 | void IRAM_ATTR loop() { 854 | int i; 855 | unsigned int temp; 856 | char ser_read; 857 | count++; 858 | if (count>99) 859 | count=0; 860 | 861 | 862 | voltage_value = analogRead(VOLT_MON); 863 | voltage = (float)voltage_value * voltage_scale; 864 | textDemo(); 865 | //delay(10); 866 | //CAN_Send(); 867 | send_esp_now(); 868 | 869 | //delay(50); 870 | 871 | GPIO.out_w1tc = ((uint32_t)1 << TEST_IO2); //TESTPINB 872 | /* 873 | //analogRead(VOLT_MON); 874 | if (SerialBT.hasClient()) 875 | Serial.print ("BT "); 876 | else 877 | Serial.print ("OFF "); 878 | 879 | Serial.print (man_offset); 880 | Serial.print (" "); 881 | Serial.println (analogRead(VOLT_MON)); 882 | */ 883 | //Serial.println (local_adc1_read(DRV_SO1)); 884 | /* 885 | Serial.print (" "); 886 | Serial.print (analogRead(local_adc1_read(DRV_SO2))); 887 | Serial.print (" "); 888 | Serial.print (analogRead(local_adc1_read(DRV_SO3))); 889 | Serial.print (" "); 890 | Serial.print (analogRead(local_adc1_read(VOLT_MON))); 891 | Serial.print (" "); 892 | Serial.print (analogRead(local_adc1_read(MOTOR_TEMP))); 893 | Serial.print (" "); 894 | Serial.println (analogRead(local_adc1_read(VOLT_MON))); 895 | */ 896 | 897 | GPIO.out_w1ts = ((uint32_t)1 << TEST_IO2); //TESTPINB 898 | 899 | if (SerialBT.available() || Serial.available() ) 900 | { 901 | //ser_read = SerialBT.read(); 902 | if (SerialBT.available()) 903 | { 904 | ser_read = SerialBT.read(); 905 | SerialBT.println(ser_read); 906 | } 907 | if (Serial.available()) 908 | { 909 | ser_read = Serial.read(); 910 | Serial.println(ser_read); 911 | } 912 | 913 | if (ser_read=='l') 914 | { 915 | man_offset++; 916 | } 917 | else if(ser_read=='k') 918 | { 919 | man_offset--; 920 | } 921 | else if(ser_read=='q') 922 | { 923 | Motor_on=0; 924 | GPIO.out_w1tc= ((uint32_t)1 << DRV_EN_GATE); 925 | } 926 | else if(ser_read=='w') 927 | { 928 | fault=0; 929 | delay(10); 930 | for (i=0; i<15;i++) 931 | { 932 | temp=DRV8305_SPI_Read(i); 933 | Serial.print(i); 934 | Serial.print(" "); 935 | Serial.println(temp, HEX); 936 | } 937 | fault=1; 938 | Motor_on=1; 939 | GPIO.out_w1ts= ((uint32_t)1 << DRV_EN_GATE); 940 | } 941 | 942 | else if (ser_read=='b') 943 | { 944 | Motor_Abs=0; 945 | } 946 | 947 | else if (ser_read=='a') 948 | { 949 | Tar_Position=-8000; 950 | //forward=0; 951 | } 952 | else if (ser_read=='s') 953 | { 954 | Tar_Position=0; 955 | //forward=1; 956 | } 957 | else if (ser_read=='d') 958 | { 959 | Tar_Position=8000; 960 | //forward=1; 961 | } 962 | else if (ser_read=='f') 963 | { 964 | fault=0; 965 | delay(10); 966 | for (i=0; i<15;i++) 967 | { 968 | temp=DRV8305_SPI_Read(i); 969 | Serial.print(i); 970 | Serial.print(" "); 971 | Serial.println(temp, HEX); 972 | } 973 | //Serial4_DRV_Write(7, 0x296); 974 | fault=1; 975 | } 976 | 977 | } 978 | 979 | } 980 | -------------------------------------------------------------------------------- /Brushless ESP-32.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/gouldpa/ESP-32-Brushless/1a5f071dd78ed1876c307b0c4e4a2c44fcf87ba8/Brushless ESP-32.pdf -------------------------------------------------------------------------------- /CAN_TEST.ino: -------------------------------------------------------------------------------- 1 | #include "BluetoothSerial.h" 2 | 3 | #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) 4 | #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it 5 | #endif 6 | 7 | BluetoothSerial SerialBT; 8 | #include 9 | // CAN Pins (Default ???) can also be spare UART 10 | #define CAN_RX 16 11 | #define CAN_TX 17 12 | // I2C Pins (Default) 13 | #define I2C_DATA 21 14 | #define I2C_CLK 22 15 | // UART Pins Default 16 | #define TXD0 1 17 | #define RXD0 3 18 | // Test Pins 19 | 20 | #define TEST_IO2 23 21 | #define TEST_IO4 25 22 | #define VOLT_MON 39 23 | 24 | #define buf_siz 60 25 | char comms_buf[buf_siz]; 26 | 27 | 28 | 29 | 30 | void IRAM_ATTR CAN_Send(int id, int stat, int motor_tar_pos, int Motor_PWM_Max){ 31 | //CAN.beginPacket(0x12); 32 | CAN.beginPacket(id); 33 | 34 | CAN.write((int)(stat >>8) & 0x00FF); 35 | CAN.write(((int)(stat) & 0x00FF)); 36 | CAN.write((int)(motor_tar_pos >>8) & 0x00FF); 37 | CAN.write(((int)(motor_tar_pos) & 0x00FF)); 38 | CAN.write((int)(Motor_PWM_Max >>8) & 0x00FF); 39 | CAN.write(((int)(Motor_PWM_Max) & 0x00FF)); 40 | 41 | CAN.endPacket(); 42 | } 43 | 44 | void IRAM_ATTR CAN_Init(){ 45 | CAN.setPins(CAN_RX, CAN_TX); 46 | Serial.println("CAN Sender"); 47 | 48 | // start the CAN bus at 500 kbps 49 | if (!CAN.begin(500E3)) { 50 | Serial.println("Starting CAN failed!"); 51 | //while (1); 52 | } 53 | CAN.setPins(CAN_RX, CAN_TX); 54 | 55 | //CAN.onReceive(onReceive); 56 | } 57 | 58 | double fls_as; 59 | double flh_as; 60 | double flk_as; 61 | double frs_as; 62 | double frh_as; 63 | double frk_as; 64 | 65 | double rls_as; 66 | double rlh_as; 67 | double rlk_as; 68 | double rrs_as; 69 | double rrh_as; 70 | double rrk_as; 71 | 72 | 73 | int flsn = 12000; 74 | int fls9 = 0; 75 | int flhn = 12000; 76 | int flh9 = 0; 77 | int flkn = 17000; 78 | int flk9 = 7000; 79 | /// //////////////////// 80 | int frsn = 12000; 81 | int frs9 = 0; 82 | int frhn = 12000; 83 | int frh9 = 0; 84 | int frkn = 17000; 85 | int frk9 = 7000; 86 | 87 | int rlsn = 12000; 88 | int rls9 = 0; 89 | int rlhn = 12000; 90 | int rlh9 = 0; 91 | int rlkn = 17000; 92 | int rlk9 = 7000; 93 | /// //////////////////// 94 | int rrsn = 12000; 95 | int rrs9 = 0; 96 | int rrhn = 12000; 97 | int rrh9 = 0; 98 | int rrkn = 17000; 99 | int rrk9 = 7000; 100 | 101 | void setup() { 102 | Serial.begin(115200); 103 | SerialBT.begin("irq_BT"); //Bluetooth device name 104 | //SerialBT.setTimeout(2); 105 | //Serial.println("The device started, now you can pair it with bluetooth!"); 106 | 107 | pinMode(TEST_IO2, OUTPUT); 108 | 109 | fls_as = (double)(flsn - fls9) / 90.0; 110 | flh_as = (double)(flhn - flh9) / 90.0; 111 | flk_as = (double)(flkn - flk9) / -90.0; 112 | frs_as = (double)(frsn - frs9) / 90.0; 113 | frh_as = (double)(frhn - frh9) / 90.0; 114 | frk_as = (double)(frkn - frk9) / -90.0; 115 | 116 | rls_as = (double)(rlsn - rls9) / 90.0; 117 | rlh_as = (double)(rlhn - rlh9) / 90.0; 118 | rlk_as = (double)(rlkn - rlk9) / -90.0; 119 | rrs_as = (double)(rrsn - rrs9) / 90.0; 120 | rrh_as = (double)(rrhn - rrh9) / 90.0; 121 | rrk_as = (double)(rrkn - rrk9) / -90.0; 122 | 123 | CAN_Init(); 124 | delay(100); 125 | CAN_Send(10, 0, 0, 0); 126 | CAN_Send(11, 0, 0, 0); 127 | CAN_Send(12, 0, 0, 0); 128 | CAN_Send(20, 0, 0, 0); 129 | CAN_Send(21, 0, 0, 0); 130 | CAN_Send(22, 0, 0, 0); 131 | CAN_Send(30, 0, 0, 0); 132 | CAN_Send(31, 0, 0, 0); 133 | CAN_Send(32, 0, 0, 0); 134 | CAN_Send(40, 0, 0, 0); 135 | CAN_Send(41, 0, 0, 0); 136 | CAN_Send(42, 0, 0, 0); 137 | delay(100); 138 | 139 | 140 | } 141 | #define LC 4 142 | double HIP_LEN = 20.0; 143 | double THY_LEN = 110.0; 144 | double SHIN_LEN = 135; 145 | double rad_deg = 180.0 / 3.1415; 146 | double deg_rad = 3.1415 / 180.0; 147 | int hipx[LC]= { 300, 300, 500, 500 }; 148 | int hipy[LC]= { 100, 300, 100, 300 }; 149 | int hipz[LC]; 150 | int kneex [LC]; 151 | int kneey [LC]; 152 | int kneez [LC]; 153 | int footx [LC]; 154 | int footy [LC]; 155 | int footz [LC]; 156 | 157 | 158 | 159 | double hip_foot_dist = 0; 160 | //double[] hip_foot_ang = new double[LC]; 161 | double hip_foot_ang = 0; 162 | double hip_foot_ang_deg = 0; 163 | //double hip_footyz_ang = 0; 164 | double hip_footyz_ang [LC]; 165 | //double hip_ang = 0; 166 | double hip_ang [LC]; 167 | double hip_ang_deg = 0; 168 | //double knee_ang = 0; 169 | double knee_ang [LC]; 170 | double knee_ang_deg = 0; 171 | double hip_knee_ang = 0; 172 | double hip_knee_ang_deg = 0; 173 | 174 | int s_ang [LC]; 175 | int h_ang [LC]; 176 | int k_ang [LC]; 177 | 178 | int good_leg = 0; 179 | int started = 0; 180 | int wheel = 0; 181 | int counter = 0; 182 | //int speed = 0; 183 | 184 | byte walking = false; 185 | int walk_counter = 0; 186 | double walk_cnt_max =500.0; 187 | double leg_phase [LC]; 188 | 189 | int sfootx [LC]; 190 | int sfooty [LC]; 191 | int sfootz[LC]; 192 | 193 | void Ang_To_Pos() 194 | { 195 | /////// left left left left left 196 | int u = 0; 197 | s_ang[u] = ((int)((hip_footyz_ang[u] * rad_deg) * -fls_as) + flsn); 198 | 199 | h_ang[u] = ((int)((hip_ang[u] * rad_deg) * -flh_as) + flhn); 200 | 201 | k_ang[u] = ((int)(((180.0 - (knee_ang[u] * rad_deg)) *flk_as) + flkn)); 202 | 203 | ///// right right right right right 204 | u = 1; 205 | s_ang[u] = -((int)((hip_footyz_ang[u] * rad_deg) * -frs_as) + frsn); 206 | 207 | h_ang[u] = -((int)((hip_ang[u] * rad_deg) * -frh_as) + frhn); 208 | 209 | k_ang[u] = -((int)(((180.0 - (knee_ang[u] * rad_deg)) * frk_as) + frkn)); 210 | 211 | ////////////////////////////////////////////////////////////////// 212 | // rear 213 | u = 2; 214 | s_ang[u] = ((int)((hip_footyz_ang[u] * rad_deg) * -rls_as) + rlsn); 215 | 216 | h_ang[u] = ((int)((hip_ang[u] * rad_deg) * -rlh_as) + rlhn); 217 | 218 | k_ang[u] = ((int)(((180.0 - (knee_ang[u] * rad_deg)) *rlk_as) + rlkn)); 219 | 220 | ///// right right right right right 221 | u = 3; 222 | s_ang[u] = -((int)((hip_footyz_ang[u] * rad_deg) * -rrs_as) + rrsn); 223 | 224 | h_ang[u] = -((int)((hip_ang[u] * rad_deg) * -rrh_as) + rrhn); 225 | 226 | k_ang[u] = -((int)(((180.0 - (knee_ang[u] * rad_deg)) * rrk_as) + rrkn)); 227 | 228 | 229 | } 230 | void Cal_Leg3D(int u) 231 | { 232 | double hip_foot_distyz = (sqrt((double)(sfootz[u] * sfootz[u] + sfooty[u] * sfooty[u]))); 233 | hip_footyz_ang[u] = atan((double)sfootz[u] / (double)sfooty[u]); 234 | double hip_footyz_ang_deg = hip_footyz_ang[u] * rad_deg; 235 | //hip_side_lbl.Text = hip_footyz_ang_deg.ToString(); 236 | double hip_foot_j_distyz = hip_foot_distyz - HIP_LEN; 237 | 238 | hip_foot_dist = (sqrt((double)(sfootx[u] * sfootx[u] + hip_foot_j_distyz * hip_foot_j_distyz))); 239 | if ((hip_foot_dist < (THY_LEN + SHIN_LEN)) && (hip_foot_dist > (SHIN_LEN - THY_LEN))) 240 | { 241 | hip_foot_ang = atan((double)sfootx[u] / (double)hip_foot_j_distyz); 242 | hip_foot_ang_deg = hip_foot_ang * rad_deg; 243 | //hip_foot_ang_lbl.Text = hip_foot_ang_deg.ToString(); 244 | //hip_foot_dist_lbl.Text = hip_foot_dist.ToString(); 245 | knee_ang[u] = acos((double)(SHIN_LEN * SHIN_LEN + THY_LEN * THY_LEN - hip_foot_dist * hip_foot_dist) / ((double)(2.0 * SHIN_LEN * THY_LEN))); 246 | knee_ang_deg = knee_ang[u] * rad_deg; 247 | //knee_ang_lbl.Text = knee_ang_deg.ToString(); 248 | hip_knee_ang = acos((double)(THY_LEN * THY_LEN + hip_foot_dist * hip_foot_dist - SHIN_LEN * SHIN_LEN) / ((double)(2.0 * THY_LEN * hip_foot_dist))); 249 | hip_knee_ang_deg = hip_knee_ang * rad_deg; 250 | //hip_knee_ang_lbl.Text = hip_knee_ang_deg.ToString(); 251 | hip_ang[u] = hip_foot_ang + hip_knee_ang; 252 | hip_ang_deg = hip_ang[u] * rad_deg; 253 | //hip_ang_lbl.Text = hip_ang_deg.ToString(); 254 | kneex[u] = (int)(sin(hip_ang[u]) * THY_LEN); 255 | kneey[u] = (int)(cos(hip_ang[u]) * THY_LEN); 256 | good_leg = 1; 257 | } 258 | else 259 | { 260 | good_leg = 0; 261 | } 262 | } 263 | 264 | double TOG = 0.1; 265 | double Base_Height = 215.0; 266 | double Foot_Off_Ground = 50.0; 267 | double Stride_length=70.0; 268 | double l_r=0.0; 269 | double Stride_length_a[LC]; 270 | double Stride_offset = 0.0; 271 | 272 | void walk() 273 | { 274 | leg_phase[0] = (double) walk_counter; 275 | leg_phase[1] = (double)walk_counter + 0.5 * walk_cnt_max; 276 | leg_phase[2] = (double)walk_counter + 0.5 * walk_cnt_max; 277 | leg_phase[3] = (double)walk_counter - 0.0 * walk_cnt_max; 278 | int leg; 279 | for (leg = 0; leg < LC; leg++) 280 | { 281 | if (leg_phase[leg] > walk_cnt_max) 282 | { 283 | leg_phase[leg] -= walk_cnt_max; 284 | } 285 | else if (leg_phase[leg] < 0) 286 | { 287 | leg_phase[leg] += walk_cnt_max; 288 | } 289 | } 290 | if (l_r<0.0) 291 | { 292 | Stride_length_a[0] = Stride_length*(1.0+l_r); 293 | Stride_length_a[1] = Stride_length*(1.0); 294 | Stride_length_a[2] = Stride_length*(1.0+l_r); 295 | Stride_length_a[3] = Stride_length*(1.0); 296 | } 297 | else 298 | { 299 | Stride_length_a[0] = Stride_length*(1.0); 300 | Stride_length_a[1] = Stride_length*(1.0-l_r); 301 | Stride_length_a[2] = Stride_length*(1.0); 302 | Stride_length_a[3] = Stride_length*(1.0-l_r); 303 | } 304 | 305 | double cycle; 306 | 307 | for (leg = 0; leg < LC; leg++) 308 | //leg = 0; 309 | { 310 | if ((leg_phase[leg] / (double)walk_cnt_max)< TOG ) 311 | { 312 | cycle = ((double)leg_phase[leg] / ((double)walk_cnt_max*TOG)) * 3.1415; 313 | footx[leg] = (int)(Stride_length_a[leg]*0.5 * cos(cycle)) + (int)Stride_offset; 314 | footy[leg] = (int)Base_Height - (int)(Foot_Off_Ground * sin(cycle)); 315 | } 316 | else 317 | { 318 | cycle = (((double)walk_cnt_max-(double)leg_phase[leg]) / ((double)walk_cnt_max * (1.0-TOG))); 319 | footx[leg] = (int)(-Stride_length_a[leg] * (cycle-0.5)) + (int)Stride_offset; 320 | footy[leg] = (int)Base_Height; 321 | } 322 | 323 | footz[leg] = 40; 324 | } 325 | 326 | } 327 | 328 | int pwm_set = 60; 329 | #define SLOW_PWM 60 330 | int mov=0; 331 | int tar; 332 | int cur; 333 | int sp=500; 334 | #define TILT 3000 335 | 336 | 337 | void IRAM_ATTR loop() { 338 | byte c; 339 | int leg_cnt; 340 | 341 | if (mov) 342 | { 343 | walk_cnt_max = sp; 344 | walk_counter++; 345 | if (walk_counter > walk_cnt_max) 346 | walk_counter = 0; 347 | 348 | walk(); 349 | 350 | for (leg_cnt = 0; leg_cnt50) 567 | Stride_offset=50; 568 | SerialBT.print("Stride_length "); 569 | SerialBT.println(Stride_length); 570 | } 571 | else if (c=='l') 572 | { 573 | Stride_offset=Stride_offset-2; 574 | if (Stride_offset<-50) 575 | Stride_offset=-50; 576 | SerialBT.print("Stride_offset "); 577 | SerialBT.println(Stride_offset); 578 | } 579 | else if (c=='k') 580 | { 581 | Stride_offset=Stride_offset+2; 582 | if (Stride_offset>50) 583 | Stride_offset=50; 584 | SerialBT.print("Stride_offset "); 585 | SerialBT.println(Stride_offset); 586 | } 587 | else if (c=='j') 588 | { 589 | TOG=TOG-0.05; 590 | if (TOG<0) 591 | TOG=0; 592 | SerialBT.print("TOG "); 593 | SerialBT.println(TOG); 594 | } 595 | else if (c=='h') 596 | { 597 | TOG=TOG+.05; 598 | if (TOG>1) 599 | TOG=1; 600 | SerialBT.print("TOG "); 601 | SerialBT.println(TOG); 602 | } 603 | else if (c=='g') 604 | { 605 | Base_Height=Base_Height-5; 606 | if (Base_Height<100) 607 | Base_Height=100; 608 | SerialBT.print("Base_Height "); 609 | SerialBT.println(Base_Height); 610 | } 611 | else if (c=='f') 612 | { 613 | Base_Height=Base_Height+5; 614 | if (Base_Height>250) 615 | Base_Height=250; 616 | SerialBT.print("Base_Height "); 617 | SerialBT.println(Base_Height); 618 | } 619 | else if (c=='i') 620 | { 621 | Foot_Off_Ground=Foot_Off_Ground-5; 622 | if (Foot_Off_Ground<0) 623 | Foot_Off_Ground=0; 624 | SerialBT.print("Foot_Off_Ground "); 625 | SerialBT.println(Foot_Off_Ground); 626 | } 627 | else if (c=='u') 628 | { 629 | Foot_Off_Ground=Foot_Off_Ground+5; 630 | if (Foot_Off_Ground>100) 631 | Foot_Off_Ground=100; 632 | SerialBT.print("Foot_Off_Ground "); 633 | SerialBT.println(Foot_Off_Ground); 634 | } 635 | else if (c=='z') 636 | { 637 | l_r=-0.5; 638 | SerialBT.println("left "); 639 | SerialBT.println(l_r); 640 | } 641 | else if (c=='x') 642 | { 643 | l_r=0; 644 | SerialBT.print("straight "); 645 | SerialBT.println(l_r); 646 | } 647 | else if (c=='c') 648 | { 649 | l_r=0.5; 650 | SerialBT.print("right "); 651 | SerialBT.println(l_r); 652 | } 653 | 654 | } 655 | } 656 | -------------------------------------------------------------------------------- /QUAD_BRAIN_CAN.ino: -------------------------------------------------------------------------------- 1 | #include "BluetoothSerial.h" 2 | 3 | #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) 4 | #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it 5 | #endif 6 | 7 | BluetoothSerial SerialBT; 8 | #include 9 | // CAN Pins (Default ???) can also be spare UART 10 | #define CAN_RX 16 11 | #define CAN_TX 17 12 | // I2C Pins (Default) 13 | #define I2C_DATA 21 14 | #define I2C_CLK 22 15 | // UART Pins Default 16 | #define TXD0 1 17 | #define RXD0 3 18 | // Test Pins 19 | 20 | #define TEST_IO2 23 21 | #define TEST_IO4 25 22 | #define VOLT_MON 39 23 | 24 | #define buf_siz 60 25 | char comms_buf[buf_siz]; 26 | 27 | 28 | 29 | 30 | void IRAM_ATTR CAN_Send(int id, int stat, int motor_tar_pos, int Motor_PWM_Max){ 31 | //CAN.beginPacket(0x12); 32 | CAN.beginPacket(id); 33 | 34 | CAN.write((int)(stat >>8) & 0x00FF); 35 | CAN.write(((int)(stat) & 0x00FF)); 36 | CAN.write((int)(motor_tar_pos >>8) & 0x00FF); 37 | CAN.write(((int)(motor_tar_pos) & 0x00FF)); 38 | CAN.write((int)(Motor_PWM_Max >>8) & 0x00FF); 39 | CAN.write(((int)(Motor_PWM_Max) & 0x00FF)); 40 | 41 | CAN.endPacket(); 42 | } 43 | 44 | void IRAM_ATTR CAN_Init(){ 45 | CAN.setPins(CAN_RX, CAN_TX); 46 | Serial.println("CAN Sender"); 47 | 48 | // start the CAN bus at 500 kbps 49 | if (!CAN.begin(500E3)) { 50 | Serial.println("Starting CAN failed!"); 51 | //while (1); 52 | } 53 | CAN.setPins(CAN_RX, CAN_TX); 54 | 55 | //CAN.onReceive(onReceive); 56 | } 57 | 58 | double fls_as; 59 | double flh_as; 60 | double flk_as; 61 | double frs_as; 62 | double frh_as; 63 | double frk_as; 64 | 65 | 66 | int flsn = 12000; 67 | int fls9 = 0; 68 | int flhn = 12000; 69 | int flh9 = 0; 70 | int flkn = 17000; 71 | int flk9 = 7000; 72 | /// //////////////////// 73 | int frsn = 12000; 74 | int frs9 = 0; 75 | int frhn = 12000; 76 | int frh9 = 0; 77 | int frkn = 17000; 78 | int frk9 = 7000; 79 | 80 | void setup() { 81 | Serial.begin(115200); 82 | SerialBT.begin("Quad_BT"); //Bluetooth device name 83 | //SerialBT.setTimeout(2); 84 | //Serial.println("The device started, now you can pair it with bluetooth!"); 85 | 86 | pinMode(TEST_IO2, OUTPUT); 87 | 88 | fls_as = (double)(flsn - fls9) / 90.0; 89 | flh_as = (double)(flhn - flh9) / 90.0; 90 | flk_as = (double)(flkn - flk9) / -90.0; 91 | frs_as = (double)(frsn - frs9) / 90.0; 92 | frh_as = (double)(frhn - frh9) / 90.0; 93 | frk_as = (double)(frkn - frk9) / -90.0; 94 | 95 | CAN_Init(); 96 | delay(100); 97 | CAN_Send(10, 0, 0, 0); 98 | CAN_Send(11, 0, 0, 0); 99 | CAN_Send(12, 0, 0, 0); 100 | CAN_Send(20, 0, 0, 0); 101 | CAN_Send(21, 0, 0, 0); 102 | CAN_Send(22, 0, 0, 0); 103 | delay(100); 104 | } 105 | #define LC 4 106 | double HIP_LEN = 20.0; 107 | double THY_LEN = 110.0; 108 | double SHIN_LEN = 135; 109 | double rad_deg = 180.0 / 3.1415; 110 | double deg_rad = 3.1415 / 180.0; 111 | int hipx[LC]= { 300, 300, 500, 500 }; 112 | int hipy[LC]= { 100, 300, 100, 300 }; 113 | int hipz[LC]; 114 | int kneex [LC]; 115 | int kneey [LC]; 116 | int kneez [LC]; 117 | int footx [LC]; 118 | int footy [LC]; 119 | int footz [LC]; 120 | 121 | 122 | 123 | double hip_foot_dist = 0; 124 | //double[] hip_foot_ang = new double[LC]; 125 | double hip_foot_ang = 0; 126 | double hip_foot_ang_deg = 0; 127 | //double hip_footyz_ang = 0; 128 | double hip_footyz_ang [LC]; 129 | //double hip_ang = 0; 130 | double hip_ang [LC]; 131 | double hip_ang_deg = 0; 132 | //double knee_ang = 0; 133 | double knee_ang [LC]; 134 | double knee_ang_deg = 0; 135 | double hip_knee_ang = 0; 136 | double hip_knee_ang_deg = 0; 137 | 138 | int s_ang [LC]; 139 | int h_ang [LC]; 140 | int k_ang [LC]; 141 | 142 | int good_leg = 0; 143 | int started = 0; 144 | int wheel = 0; 145 | int counter = 0; 146 | //int speed = 0; 147 | 148 | byte walking = false; 149 | int walk_counter = 0; 150 | double walk_cnt_max =500.0; 151 | double leg_phase [LC]; 152 | 153 | int sfootx [LC]; 154 | int sfooty [LC]; 155 | int sfootz[LC]; 156 | 157 | void Ang_To_Pos() 158 | { 159 | /////// left left left left left 160 | int u = 0; 161 | s_ang[u] = ((int)((hip_footyz_ang[u] * rad_deg) * -fls_as) + flsn); 162 | 163 | h_ang[u] = ((int)((hip_ang[u] * rad_deg) * -flh_as) + flhn); 164 | 165 | k_ang[u] = ((int)(((180.0 - (knee_ang[u] * rad_deg)) *flk_as) + flkn)); 166 | 167 | ///// right right right right right 168 | u = 1; 169 | s_ang[u] = -((int)((hip_footyz_ang[u] * rad_deg) * -frs_as) + frsn); 170 | 171 | h_ang[u] = -((int)((hip_ang[u] * rad_deg) * -frh_as) + frhn); 172 | 173 | k_ang[u] = -((int)(((180.0 - (knee_ang[u] * rad_deg)) * frk_as) + frkn)); 174 | 175 | 176 | } 177 | void Cal_Leg3D(int u) 178 | { 179 | double hip_foot_distyz = (sqrt((double)(sfootz[u] * sfootz[u] + sfooty[u] * sfooty[u]))); 180 | hip_footyz_ang[u] = atan((double)sfootz[u] / (double)sfooty[u]); 181 | double hip_footyz_ang_deg = hip_footyz_ang[u] * rad_deg; 182 | //hip_side_lbl.Text = hip_footyz_ang_deg.ToString(); 183 | double hip_foot_j_distyz = hip_foot_distyz - HIP_LEN; 184 | 185 | hip_foot_dist = (sqrt((double)(sfootx[u] * sfootx[u] + hip_foot_j_distyz * hip_foot_j_distyz))); 186 | if ((hip_foot_dist < (THY_LEN + SHIN_LEN)) && (hip_foot_dist > (SHIN_LEN - THY_LEN))) 187 | { 188 | hip_foot_ang = atan((double)sfootx[u] / (double)hip_foot_j_distyz); 189 | hip_foot_ang_deg = hip_foot_ang * rad_deg; 190 | //hip_foot_ang_lbl.Text = hip_foot_ang_deg.ToString(); 191 | //hip_foot_dist_lbl.Text = hip_foot_dist.ToString(); 192 | knee_ang[u] = acos((double)(SHIN_LEN * SHIN_LEN + THY_LEN * THY_LEN - hip_foot_dist * hip_foot_dist) / ((double)(2.0 * SHIN_LEN * THY_LEN))); 193 | knee_ang_deg = knee_ang[u] * rad_deg; 194 | //knee_ang_lbl.Text = knee_ang_deg.ToString(); 195 | hip_knee_ang = acos((double)(THY_LEN * THY_LEN + hip_foot_dist * hip_foot_dist - SHIN_LEN * SHIN_LEN) / ((double)(2.0 * THY_LEN * hip_foot_dist))); 196 | hip_knee_ang_deg = hip_knee_ang * rad_deg; 197 | //hip_knee_ang_lbl.Text = hip_knee_ang_deg.ToString(); 198 | hip_ang[u] = hip_foot_ang + hip_knee_ang; 199 | hip_ang_deg = hip_ang[u] * rad_deg; 200 | //hip_ang_lbl.Text = hip_ang_deg.ToString(); 201 | kneex[u] = (int)(sin(hip_ang[u]) * THY_LEN); 202 | kneey[u] = (int)(cos(hip_ang[u]) * THY_LEN); 203 | good_leg = 1; 204 | } 205 | else 206 | { 207 | good_leg = 0; 208 | } 209 | } 210 | 211 | void walk() 212 | { 213 | leg_phase[0] = (double) walk_counter; 214 | leg_phase[1] = (double)walk_counter + 0.5 * walk_cnt_max; 215 | leg_phase[2] = (double)walk_counter + 0.5 * walk_cnt_max; 216 | leg_phase[3] = (double)walk_counter - 0.0 * walk_cnt_max; 217 | int leg; 218 | for (leg = 0; leg < LC; leg++) 219 | { 220 | if (leg_phase[leg] > walk_cnt_max) 221 | { 222 | leg_phase[leg] -= walk_cnt_max; 223 | } 224 | else if (leg_phase[leg] < 0) 225 | { 226 | leg_phase[leg] += walk_cnt_max; 227 | } 228 | } 229 | 230 | double TOG = 0.3; 231 | double Base_Height = 200.0; 232 | double Foot_Off_Ground = 50.0; 233 | double Stride_length = 100.0; 234 | double Stride_offset = 0.0; 235 | 236 | double cycle; 237 | 238 | for (leg = 0; leg < LC; leg++) 239 | //leg = 0; 240 | { 241 | if ((leg_phase[leg] / (double)walk_cnt_max)< TOG ) 242 | { 243 | cycle = ((double)leg_phase[leg] / ((double)walk_cnt_max*TOG)) * 3.1415; 244 | footx[leg] = (int)(Stride_length*0.5 * cos(cycle)) + (int)Stride_offset; 245 | footy[leg] = (int)Base_Height - (int)(Foot_Off_Ground * sin(cycle)); 246 | } 247 | else 248 | { 249 | cycle = (((double)walk_cnt_max-(double)leg_phase[leg]) / ((double)walk_cnt_max * (1.0-TOG))); 250 | footx[leg] = (int)(-Stride_length * (cycle-0.5)) + (int)Stride_offset; 251 | footy[leg] = (int)Base_Height; 252 | } 253 | 254 | footz[leg] = 40; 255 | } 256 | 257 | } 258 | 259 | #define PWM_SET 250 260 | #define SLOW_PWM 70 261 | int mov=0; 262 | int tar; 263 | int cur; 264 | int sp=50; 265 | 266 | 267 | void IRAM_ATTR loop() { 268 | byte c; 269 | int leg_cnt; 270 | 271 | if (mov) 272 | { 273 | walk_counter++; 274 | if (walk_counter > walk_cnt_max) 275 | walk_counter = 0; 276 | 277 | walk(); 278 | 279 | for (leg_cnt = 0; leg_cnt 9 | // CAN Pins (Default ???) can also be spare UART 10 | #define CAN_RX 16 11 | #define CAN_TX 17 12 | // I2C Pins (Default) 13 | #define I2C_DATA 21 14 | #define I2C_CLK 22 15 | // UART Pins Default 16 | #define TXD0 1 17 | #define RXD0 3 18 | // Test Pins 19 | 20 | #define TEST_IO2 23 21 | #define TEST_IO4 25 22 | #define VOLT_MON 39 23 | 24 | #define buf_siz 60 25 | char comms_buf[buf_siz]; 26 | 27 | 28 | 29 | 30 | void IRAM_ATTR CAN_Send(int id, int stat, int motor_tar_pos, int Motor_PWM_Max){ 31 | //CAN.beginPacket(0x12); 32 | CAN.beginPacket(id); 33 | 34 | CAN.write((int)(stat >>8) & 0x00FF); 35 | CAN.write(((int)(stat) & 0x00FF)); 36 | CAN.write((int)(motor_tar_pos >>8) & 0x00FF); 37 | CAN.write(((int)(motor_tar_pos) & 0x00FF)); 38 | CAN.write((int)(Motor_PWM_Max >>8) & 0x00FF); 39 | CAN.write(((int)(Motor_PWM_Max) & 0x00FF)); 40 | 41 | CAN.endPacket(); 42 | } 43 | 44 | void IRAM_ATTR CAN_Init(){ 45 | CAN.setPins(CAN_RX, CAN_TX); 46 | Serial.println("CAN Sender"); 47 | 48 | // start the CAN bus at 500 kbps 49 | if (!CAN.begin(500E3)) { 50 | Serial.println("Starting CAN failed!"); 51 | //while (1); 52 | } 53 | CAN.setPins(CAN_RX, CAN_TX); 54 | 55 | //CAN.onReceive(onReceive); 56 | } 57 | 58 | double fls_as; 59 | double flh_as; 60 | double flk_as; 61 | double frs_as; 62 | double frh_as; 63 | double frk_as; 64 | 65 | double rls_as; 66 | double rlh_as; 67 | double rlk_as; 68 | double rrs_as; 69 | double rrh_as; 70 | double rrk_as; 71 | 72 | 73 | double flsn = 12000; 74 | double fls9 = 0; 75 | double flhn = 12000; 76 | double flh9 = 0; 77 | double flkn = 17000; 78 | double flk9 = 7000; 79 | /// //////////////////// 80 | double frsn = 12000; 81 | double frs9 = 0; 82 | double frhn = 12000; 83 | double frh9 = 0; 84 | double frkn = 17000; 85 | double frk9 = 7000; 86 | 87 | double rlsn = 12000; 88 | double rls9 = 0; 89 | double rlhn = 12000; 90 | double rlh9 = 0; 91 | double rlkn = 17000; 92 | double rlk9 = 7000; 93 | /// //////////////////// 94 | double rrsn = 12000; 95 | double rrs9 = 0; 96 | double rrhn = 12000; 97 | double rrh9 = 0; 98 | double rrkn = 17000; 99 | double rrk9 = 7000; 100 | 101 | void setup() { 102 | Serial.begin(115200); 103 | SerialBT.begin("Quad_BT"); //Bluetooth device name 104 | //SerialBT.setTimeout(2); 105 | //Serial.println("The device started, now you can pair it with bluetooth!"); 106 | 107 | pinMode(TEST_IO2, OUTPUT); 108 | 109 | fls_as = (double)(flsn - fls9) / 90.0; 110 | flh_as = (double)(flhn - flh9) / 90.0; 111 | flk_as = (double)(flkn - flk9) / -90.0; 112 | frs_as = (double)(frsn - frs9) / 90.0; 113 | frh_as = (double)(frhn - frh9) / 90.0; 114 | frk_as = (double)(frkn - frk9) / -90.0; 115 | 116 | rls_as = (double)(rlsn - rls9) / 90.0; 117 | rlh_as = (double)(rlhn - rlh9) / 90.0; 118 | rlk_as = (double)(rlkn - rlk9) / -90.0; 119 | rrs_as = (double)(rrsn - rrs9) / 90.0; 120 | rrh_as = (double)(rrhn - rrh9) / 90.0; 121 | rrk_as = (double)(rrkn - rrk9) / -90.0; 122 | 123 | CAN_Init(); 124 | delay(100); 125 | CAN_Send(10, 0, 0, 0); 126 | CAN_Send(11, 0, 0, 0); 127 | CAN_Send(12, 0, 0, 0); 128 | CAN_Send(20, 0, 0, 0); 129 | CAN_Send(21, 0, 0, 0); 130 | CAN_Send(22, 0, 0, 0); 131 | CAN_Send(30, 0, 0, 0); 132 | CAN_Send(31, 0, 0, 0); 133 | CAN_Send(32, 0, 0, 0); 134 | CAN_Send(40, 0, 0, 0); 135 | CAN_Send(41, 0, 0, 0); 136 | CAN_Send(42, 0, 0, 0); 137 | delay(100); 138 | 139 | 140 | } 141 | #define LC 4 142 | double HIP_LEN = 20.0; 143 | double THY_LEN = 110.0; 144 | double SHIN_LEN = 135.0; 145 | double rad_deg = 180.0 / 3.1415; 146 | double deg_rad = 3.1415 / 180.0; 147 | double hipx[LC]= { 300, 300, 500, 500 }; 148 | double hipy[LC]= { 100, 300, 100, 300 }; 149 | double hipz[LC]; 150 | double kneex [LC]; 151 | double kneey [LC]; 152 | double kneez [LC]; 153 | double footx [LC]; 154 | double footy [LC]; 155 | double footz [LC]; 156 | 157 | 158 | 159 | double hip_foot_dist = 0; 160 | //double[] hip_foot_ang = new double[LC]; 161 | double hip_foot_ang = 0; 162 | double hip_foot_ang_deg = 0; 163 | //double hip_footyz_ang = 0; 164 | double hip_footyz_ang [LC]; 165 | //double hip_ang = 0; 166 | double hip_ang [LC]; 167 | double hip_ang_deg = 0; 168 | //double knee_ang = 0; 169 | double knee_ang [LC]; 170 | double knee_ang_deg = 0; 171 | double hip_knee_ang = 0; 172 | double hip_knee_ang_deg = 0; 173 | 174 | double s_ang [LC]; 175 | double h_ang [LC]; 176 | double k_ang [LC]; 177 | 178 | int good_leg = 0; 179 | int started = 0; 180 | int wheel = 0; 181 | int counter = 0; 182 | //int speed = 0; 183 | 184 | byte walking = false; 185 | int walk_counter = 0; 186 | double walk_cnt_max =500.0; 187 | double leg_phase [LC]; 188 | 189 | double sfootx [LC]; 190 | double sfooty [LC]; 191 | double sfootz[LC]; 192 | 193 | void Ang_To_Pos() 194 | { 195 | /////// left left left left left 196 | int u = 0; 197 | s_ang[u] = (((hip_footyz_ang[u] * rad_deg) * -fls_as) + flsn); 198 | 199 | h_ang[u] = (((hip_ang[u] * rad_deg) * -flh_as) + flhn); 200 | 201 | k_ang[u] = ((((180.0 - (knee_ang[u] * rad_deg)) *flk_as) + flkn)); 202 | 203 | ///// right right right right right 204 | u = 1; 205 | s_ang[u] = -(((hip_footyz_ang[u] * rad_deg) * -frs_as) + frsn); 206 | 207 | h_ang[u] = -(((hip_ang[u] * rad_deg) * -frh_as) + frhn); 208 | 209 | k_ang[u] = -((((180.0 - (knee_ang[u] * rad_deg)) * frk_as) + frkn)); 210 | 211 | ////////////////////////////////////////////////////////////////// 212 | // rear 213 | u = 2; 214 | s_ang[u] = (((hip_footyz_ang[u] * rad_deg) * -rls_as) + rlsn); 215 | 216 | h_ang[u] = (((hip_ang[u] * rad_deg) * -rlh_as) + rlhn); 217 | 218 | k_ang[u] = ((((180.0 - (knee_ang[u] * rad_deg)) *rlk_as) + rlkn)); 219 | 220 | ///// right right right right right 221 | u = 3; 222 | s_ang[u] = -(((hip_footyz_ang[u] * rad_deg) * -rrs_as) + rrsn); 223 | 224 | h_ang[u] = -(((hip_ang[u] * rad_deg) * -rrh_as) + rrhn); 225 | 226 | k_ang[u] = -((((180.0 - (knee_ang[u] * rad_deg)) * rrk_as) + rrkn)); 227 | 228 | 229 | } 230 | void Cal_Leg3D(int u) 231 | { 232 | double hip_foot_distyz = (sqrt((double)(sfootz[u] * sfootz[u] + sfooty[u] * sfooty[u]))); 233 | hip_footyz_ang[u] = atan((double)sfootz[u] / (double)sfooty[u]); 234 | double hip_footyz_ang_deg = hip_footyz_ang[u] * rad_deg; 235 | //hip_side_lbl.Text = hip_footyz_ang_deg.ToString(); 236 | double hip_foot_j_distyz = hip_foot_distyz - HIP_LEN; 237 | 238 | hip_foot_dist = (sqrt((double)(sfootx[u] * sfootx[u] + hip_foot_j_distyz * hip_foot_j_distyz))); 239 | if ((hip_foot_dist < (THY_LEN + SHIN_LEN)) && (hip_foot_dist > (SHIN_LEN - THY_LEN))) 240 | { 241 | hip_foot_ang = atan((double)sfootx[u] / (double)hip_foot_j_distyz); 242 | hip_foot_ang_deg = hip_foot_ang * rad_deg; 243 | //hip_foot_ang_lbl.Text = hip_foot_ang_deg.ToString(); 244 | //hip_foot_dist_lbl.Text = hip_foot_dist.ToString(); 245 | knee_ang[u] = acos((double)(SHIN_LEN * SHIN_LEN + THY_LEN * THY_LEN - hip_foot_dist * hip_foot_dist) / ((double)(2.0 * SHIN_LEN * THY_LEN))); 246 | knee_ang_deg = knee_ang[u] * rad_deg; 247 | //knee_ang_lbl.Text = knee_ang_deg.ToString(); 248 | hip_knee_ang = acos((double)(THY_LEN * THY_LEN + hip_foot_dist * hip_foot_dist - SHIN_LEN * SHIN_LEN) / ((double)(2.0 * THY_LEN * hip_foot_dist))); 249 | hip_knee_ang_deg = hip_knee_ang * rad_deg; 250 | //hip_knee_ang_lbl.Text = hip_knee_ang_deg.ToString(); 251 | hip_ang[u] = hip_foot_ang + hip_knee_ang; 252 | hip_ang_deg = hip_ang[u] * rad_deg; 253 | //hip_ang_lbl.Text = hip_ang_deg.ToString(); 254 | kneex[u] = (sin(hip_ang[u]) * THY_LEN); 255 | kneey[u] = (cos(hip_ang[u]) * THY_LEN); 256 | good_leg = 1; 257 | } 258 | else 259 | { 260 | good_leg = 0; 261 | } 262 | } 263 | 264 | double TOG = 0.1; 265 | double Base_Height = 215.0; 266 | double Foot_Off_Ground = 50.0; 267 | double Stride_length=70.0; 268 | double l_r=0.0; 269 | double Stride_length_a[LC]; 270 | double Stride_offset = 0.0; 271 | 272 | void walk() 273 | { 274 | leg_phase[0] = (double) walk_counter; 275 | leg_phase[1] = (double)walk_counter + 0.5 * walk_cnt_max; 276 | leg_phase[2] = (double)walk_counter + 0.5 * walk_cnt_max; 277 | leg_phase[3] = (double)walk_counter - 0.0 * walk_cnt_max; 278 | int leg; 279 | for (leg = 0; leg < LC; leg++) 280 | { 281 | if (leg_phase[leg] > walk_cnt_max) 282 | { 283 | leg_phase[leg] -= walk_cnt_max; 284 | } 285 | else if (leg_phase[leg] < 0) 286 | { 287 | leg_phase[leg] += walk_cnt_max; 288 | } 289 | } 290 | if (l_r<0.0) 291 | { 292 | Stride_length_a[0] = Stride_length*(1.0+l_r); 293 | Stride_length_a[1] = Stride_length*(1.0); 294 | Stride_length_a[2] = Stride_length*(1.0+l_r); 295 | Stride_length_a[3] = Stride_length*(1.0); 296 | } 297 | else 298 | { 299 | Stride_length_a[0] = Stride_length*(1.0); 300 | Stride_length_a[1] = Stride_length*(1.0-l_r); 301 | Stride_length_a[2] = Stride_length*(1.0); 302 | Stride_length_a[3] = Stride_length*(1.0-l_r); 303 | } 304 | 305 | double cycle; 306 | 307 | for (leg = 0; leg < LC; leg++) 308 | //leg = 0; 309 | { 310 | if ((leg_phase[leg] / (double)walk_cnt_max)< TOG ) 311 | { 312 | cycle = ((double)leg_phase[leg] / ((double)walk_cnt_max*(double)TOG)) * 3.1415; 313 | footx[leg] = (Stride_length_a[leg]*0.5 * cos(cycle)) + Stride_offset; 314 | footy[leg] = Base_Height - (Foot_Off_Ground * sin(cycle)); 315 | } 316 | else 317 | { 318 | cycle = (((double)walk_cnt_max-(double)leg_phase[leg]) / ((double)walk_cnt_max * (1.0-(double)TOG))); 319 | footx[leg] = (-Stride_length_a[leg] * (cycle-0.5)) + Stride_offset; 320 | footy[leg] = Base_Height; 321 | } 322 | 323 | footz[leg] = 40; 324 | } 325 | 326 | } 327 | 328 | int pwm_set = 100; 329 | #define SLOW_PWM 100 330 | int mov=0; 331 | int tar; 332 | int cur; 333 | int sp=500; 334 | #define TILT 3000 335 | 336 | 337 | void IRAM_ATTR loop() { 338 | byte c; 339 | int leg_cnt; 340 | 341 | if (mov) 342 | { 343 | walk_cnt_max = sp; 344 | walk_counter++; 345 | if (walk_counter > walk_cnt_max) 346 | walk_counter = 0; 347 | 348 | walk(); 349 | 350 | for (leg_cnt = 0; leg_cnt50) 567 | Stride_offset=50; 568 | SerialBT.print("Stride_length "); 569 | SerialBT.println(Stride_length); 570 | } 571 | else if (c=='l') 572 | { 573 | Stride_offset=Stride_offset-2; 574 | if (Stride_offset<-50) 575 | Stride_offset=-50; 576 | SerialBT.print("Stride_offset "); 577 | SerialBT.println(Stride_offset); 578 | } 579 | else if (c=='k') 580 | { 581 | Stride_offset=Stride_offset+2; 582 | if (Stride_offset>50) 583 | Stride_offset=50; 584 | SerialBT.print("Stride_offset "); 585 | SerialBT.println(Stride_offset); 586 | } 587 | else if (c=='j') 588 | { 589 | TOG=TOG-0.05; 590 | if (TOG<0) 591 | TOG=0; 592 | SerialBT.print("TOG "); 593 | SerialBT.println(TOG); 594 | } 595 | else if (c=='h') 596 | { 597 | TOG=TOG+.05; 598 | if (TOG>1) 599 | TOG=1; 600 | SerialBT.print("TOG "); 601 | SerialBT.println(TOG); 602 | } 603 | else if (c=='g') 604 | { 605 | Base_Height=Base_Height-5; 606 | if (Base_Height<100) 607 | Base_Height=100; 608 | SerialBT.print("Base_Height "); 609 | SerialBT.println(Base_Height); 610 | } 611 | else if (c=='f') 612 | { 613 | Base_Height=Base_Height+5; 614 | if (Base_Height>250) 615 | Base_Height=250; 616 | SerialBT.print("Base_Height "); 617 | SerialBT.println(Base_Height); 618 | } 619 | else if (c=='i') 620 | { 621 | Foot_Off_Ground=Foot_Off_Ground-5; 622 | if (Foot_Off_Ground<0) 623 | Foot_Off_Ground=0; 624 | SerialBT.print("Foot_Off_Ground "); 625 | SerialBT.println(Foot_Off_Ground); 626 | } 627 | else if (c=='u') 628 | { 629 | Foot_Off_Ground=Foot_Off_Ground+5; 630 | if (Foot_Off_Ground>100) 631 | Foot_Off_Ground=100; 632 | SerialBT.print("Foot_Off_Ground "); 633 | SerialBT.println(Foot_Off_Ground); 634 | } 635 | else if (c=='z') 636 | { 637 | l_r=-0.5; 638 | SerialBT.println("left "); 639 | SerialBT.println(l_r); 640 | } 641 | else if (c=='x') 642 | { 643 | l_r=0; 644 | SerialBT.print("straight "); 645 | SerialBT.println(l_r); 646 | } 647 | else if (c=='c') 648 | { 649 | l_r=0.5; 650 | SerialBT.print("right "); 651 | SerialBT.println(l_r); 652 | } 653 | 654 | } 655 | } 656 | -------------------------------------------------------------------------------- /QUAD_BRAIN_T0.ino: -------------------------------------------------------------------------------- 1 | #include "BluetoothSerial.h" 2 | 3 | #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) 4 | #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it 5 | #endif 6 | 7 | BluetoothSerial SerialBT; 8 | #include 9 | // CAN Pins (Default ???) can also be spare UART 10 | #define CAN_RX 16 11 | #define CAN_TX 17 12 | // I2C Pins (Default) 13 | #define I2C_DATA 21 14 | #define I2C_CLK 22 15 | // UART Pins Default 16 | #define TXD0 1 17 | #define RXD0 3 18 | // Test Pins 19 | 20 | #define TEST_IO2 23 21 | #define TEST_IO4 25 22 | #define VOLT_MON 39 23 | 24 | #define buf_siz 60 25 | char comms_buf[buf_siz]; 26 | 27 | 28 | 29 | 30 | void IRAM_ATTR CAN_Send(int id, int stat, int motor_tar_pos, int Motor_PWM_Max){ 31 | //CAN.beginPacket(0x12); 32 | CAN.beginPacket(id); 33 | 34 | CAN.write((int)(stat >>8) & 0x00FF); 35 | CAN.write(((int)(stat) & 0x00FF)); 36 | CAN.write((int)(motor_tar_pos >>8) & 0x00FF); 37 | CAN.write(((int)(motor_tar_pos) & 0x00FF)); 38 | CAN.write((int)(Motor_PWM_Max >>8) & 0x00FF); 39 | CAN.write(((int)(Motor_PWM_Max) & 0x00FF)); 40 | 41 | CAN.endPacket(); 42 | } 43 | 44 | void IRAM_ATTR CAN_Init(){ 45 | CAN.setPins(CAN_RX, CAN_TX); 46 | Serial.println("CAN Sender"); 47 | 48 | // start the CAN bus at 500 kbps 49 | if (!CAN.begin(500E3)) { 50 | Serial.println("Starting CAN failed!"); 51 | //while (1); 52 | } 53 | CAN.setPins(CAN_RX, CAN_TX); 54 | 55 | //CAN.onReceive(onReceive); 56 | } 57 | 58 | double fls_as; 59 | double flh_as; 60 | double flk_as; 61 | double frs_as; 62 | double frh_as; 63 | double frk_as; 64 | 65 | double rls_as; 66 | double rlh_as; 67 | double rlk_as; 68 | double rrs_as; 69 | double rrh_as; 70 | double rrk_as; 71 | 72 | 73 | double flsn = 12000; 74 | double fls9 = 0; 75 | double flhn = 12000; 76 | double flh9 = 0; 77 | double flkn = 17000; 78 | double flk9 = 7000; 79 | /// //////////////////// 80 | double frsn = 12000; 81 | double frs9 = 0; 82 | double frhn = 12000; 83 | double frh9 = 0; 84 | double frkn = 17000; 85 | double frk9 = 7000; 86 | 87 | double rlsn = 12000; 88 | double rls9 = 0; 89 | double rlhn = 12000; 90 | double rlh9 = 0; 91 | double rlkn = 17000; 92 | double rlk9 = 7000; 93 | /// //////////////////// 94 | double rrsn = 12000; 95 | double rrs9 = 0; 96 | double rrhn = 12000; 97 | double rrh9 = 0; 98 | double rrkn = 17000; 99 | double rrk9 = 7000; 100 | 101 | void setup() { 102 | Serial.begin(115200); 103 | SerialBT.begin("Quad_BT"); //Bluetooth device name 104 | //SerialBT.setTimeout(2); 105 | //Serial.println("The device started, now you can pair it with bluetooth!"); 106 | 107 | pinMode(TEST_IO2, OUTPUT); 108 | 109 | fls_as = (double)(flsn - fls9) / 90.0; 110 | flh_as = (double)(flhn - flh9) / 90.0; 111 | flk_as = (double)(flkn - flk9) / -90.0; 112 | frs_as = (double)(frsn - frs9) / 90.0; 113 | frh_as = (double)(frhn - frh9) / 90.0; 114 | frk_as = (double)(frkn - frk9) / -90.0; 115 | 116 | rls_as = (double)(rlsn - rls9) / 90.0; 117 | rlh_as = (double)(rlhn - rlh9) / 90.0; 118 | rlk_as = (double)(rlkn - rlk9) / -90.0; 119 | rrs_as = (double)(rrsn - rrs9) / 90.0; 120 | rrh_as = (double)(rrhn - rrh9) / 90.0; 121 | rrk_as = (double)(rrkn - rrk9) / -90.0; 122 | 123 | CAN_Init(); 124 | delay(100); 125 | CAN_Send(10, 0, 0, 0); 126 | CAN_Send(11, 0, 0, 0); 127 | CAN_Send(12, 0, 0, 0); 128 | CAN_Send(20, 0, 0, 0); 129 | CAN_Send(21, 0, 0, 0); 130 | CAN_Send(22, 0, 0, 0); 131 | CAN_Send(30, 0, 0, 0); 132 | CAN_Send(31, 0, 0, 0); 133 | CAN_Send(32, 0, 0, 0); 134 | CAN_Send(40, 0, 0, 0); 135 | CAN_Send(41, 0, 0, 0); 136 | CAN_Send(42, 0, 0, 0); 137 | delay(100); 138 | 139 | 140 | } 141 | #define LC 4 142 | double HIP_LEN = 20.0; 143 | double THY_LEN = 110.0; 144 | double SHIN_LEN = 135.0; 145 | double rad_deg = 180.0 / 3.1415; 146 | double deg_rad = 3.1415 / 180.0; 147 | double hipx[LC]= { 300, 300, 500, 500 }; 148 | double hipy[LC]= { 100, 300, 100, 300 }; 149 | double hipz[LC]; 150 | double kneex [LC]; 151 | double kneey [LC]; 152 | double kneez [LC]; 153 | double footx [LC]; 154 | double footy [LC]; 155 | double footz [LC]; 156 | 157 | 158 | 159 | double hip_foot_dist = 0; 160 | //double[] hip_foot_ang = new double[LC]; 161 | double hip_foot_ang = 0; 162 | double hip_foot_ang_deg = 0; 163 | //double hip_footyz_ang = 0; 164 | double hip_footyz_ang [LC]; 165 | //double hip_ang = 0; 166 | double hip_ang [LC]; 167 | double hip_ang_deg = 0; 168 | //double knee_ang = 0; 169 | double knee_ang [LC]; 170 | double knee_ang_deg = 0; 171 | double hip_knee_ang = 0; 172 | double hip_knee_ang_deg = 0; 173 | 174 | double s_ang [LC]; 175 | double h_ang [LC]; 176 | double k_ang [LC]; 177 | 178 | int good_leg = 0; 179 | int started = 0; 180 | int wheel = 0; 181 | int counter = 0; 182 | //int speed = 0; 183 | 184 | byte walking = false; 185 | int walk_counter = 0; 186 | double walk_cnt_max =500.0; 187 | double leg_phase [LC]; 188 | 189 | double sfootx [LC]; 190 | double sfooty [LC]; 191 | double sfootz[LC]; 192 | 193 | void Ang_To_Pos() 194 | { 195 | /////// left left left left left 196 | int u = 0; 197 | s_ang[u] = (((hip_footyz_ang[u] * rad_deg) * -fls_as) + flsn); 198 | 199 | h_ang[u] = (((hip_ang[u] * rad_deg) * -flh_as) + flhn); 200 | 201 | k_ang[u] = ((((180.0 - (knee_ang[u] * rad_deg)) *flk_as) + flkn)); 202 | 203 | ///// right right right right right 204 | u = 1; 205 | s_ang[u] = -(((hip_footyz_ang[u] * rad_deg) * -frs_as) + frsn); 206 | 207 | h_ang[u] = -(((hip_ang[u] * rad_deg) * -frh_as) + frhn); 208 | 209 | k_ang[u] = -((((180.0 - (knee_ang[u] * rad_deg)) * frk_as) + frkn)); 210 | 211 | ////////////////////////////////////////////////////////////////// 212 | // rear 213 | u = 2; 214 | s_ang[u] = (((hip_footyz_ang[u] * rad_deg) * -rls_as) + rlsn); 215 | 216 | h_ang[u] = (((hip_ang[u] * rad_deg) * -rlh_as) + rlhn); 217 | 218 | k_ang[u] = ((((180.0 - (knee_ang[u] * rad_deg)) *rlk_as) + rlkn)); 219 | 220 | ///// right right right right right 221 | u = 3; 222 | s_ang[u] = -(((hip_footyz_ang[u] * rad_deg) * -rrs_as) + rrsn); 223 | 224 | h_ang[u] = -(((hip_ang[u] * rad_deg) * -rrh_as) + rrhn); 225 | 226 | k_ang[u] = -((((180.0 - (knee_ang[u] * rad_deg)) * rrk_as) + rrkn)); 227 | 228 | 229 | } 230 | void Cal_Leg3D(int u) 231 | { 232 | double hip_foot_distyz = (sqrt((double)(sfootz[u] * sfootz[u] + sfooty[u] * sfooty[u]))); 233 | hip_footyz_ang[u] = atan((double)sfootz[u] / (double)sfooty[u]); 234 | double hip_footyz_ang_deg = hip_footyz_ang[u] * rad_deg; 235 | //hip_side_lbl.Text = hip_footyz_ang_deg.ToString(); 236 | double hip_foot_j_distyz = hip_foot_distyz - HIP_LEN; 237 | 238 | hip_foot_dist = (sqrt((double)(sfootx[u] * sfootx[u] + hip_foot_j_distyz * hip_foot_j_distyz))); 239 | if ((hip_foot_dist < (THY_LEN + SHIN_LEN)) && (hip_foot_dist > (SHIN_LEN - THY_LEN))) 240 | { 241 | hip_foot_ang = atan((double)sfootx[u] / (double)hip_foot_j_distyz); 242 | hip_foot_ang_deg = hip_foot_ang * rad_deg; 243 | //hip_foot_ang_lbl.Text = hip_foot_ang_deg.ToString(); 244 | //hip_foot_dist_lbl.Text = hip_foot_dist.ToString(); 245 | knee_ang[u] = acos((double)(SHIN_LEN * SHIN_LEN + THY_LEN * THY_LEN - hip_foot_dist * hip_foot_dist) / ((double)(2.0 * SHIN_LEN * THY_LEN))); 246 | knee_ang_deg = knee_ang[u] * rad_deg; 247 | //knee_ang_lbl.Text = knee_ang_deg.ToString(); 248 | hip_knee_ang = acos((double)(THY_LEN * THY_LEN + hip_foot_dist * hip_foot_dist - SHIN_LEN * SHIN_LEN) / ((double)(2.0 * THY_LEN * hip_foot_dist))); 249 | hip_knee_ang_deg = hip_knee_ang * rad_deg; 250 | //hip_knee_ang_lbl.Text = hip_knee_ang_deg.ToString(); 251 | hip_ang[u] = hip_foot_ang + hip_knee_ang; 252 | hip_ang_deg = hip_ang[u] * rad_deg; 253 | //hip_ang_lbl.Text = hip_ang_deg.ToString(); 254 | kneex[u] = (sin(hip_ang[u]) * THY_LEN); 255 | kneey[u] = (cos(hip_ang[u]) * THY_LEN); 256 | good_leg = 1; 257 | } 258 | else 259 | { 260 | good_leg = 0; 261 | } 262 | } 263 | 264 | double TOG = 0.2; 265 | double Base_Height = 215.0; 266 | double Foot_Off_Ground = 50.0; 267 | double Stride_length=0.0; 268 | double Left_per = 1.0; 269 | double Right_per = 1.0; 270 | double Stride_length_a[LC]; 271 | double Stride_offset = 0.0; 272 | 273 | double TOG_Base = 0.2; 274 | double Base_Height_Base = 215.0; 275 | double Foot_Off_Ground_Base = 50.0; 276 | double Stride_length_Base = 70.0; 277 | double Left_per_Base = 1.0; 278 | double Right_per_Base = 1.0; 279 | double Stride_length_a_Base[LC]; 280 | double Stride_offset_Base = 0.0; 281 | 282 | double TOG_Targ = 0.2; 283 | double Base_Height_Targ = 215.0; 284 | double Foot_Off_Ground_Targ = 40.0; 285 | double Stride_length_Targ = 0.0; 286 | double Left_per_Targ = 1.0; 287 | double Right_per_Targ = 1.0; 288 | double Stride_length_a_Targ[LC]; 289 | double Stride_offset_Targ = 0.0; 290 | 291 | double TOG_Delta = 0.0; 292 | double Base_Height_Delta = 0.0; 293 | double Foot_Off_Ground_Delta = 0.0; 294 | double Stride_length_Delta = 0.0; 295 | double Left_per_Delta = 0.0; 296 | double Right_per_Delta = 0.0; 297 | double Stride_length_a_Delta[LC]; 298 | double Stride_offset_Delta = 0.0; 299 | 300 | int transition; 301 | #define TRANS_MAX 1000 302 | 303 | int steer_trans; 304 | #define STEER_MAX 500 305 | 306 | void Trans_Start() 307 | { 308 | transition = TRANS_MAX; 309 | Stride_length_Delta = (Stride_length - Stride_length_Targ) / TRANS_MAX; 310 | } 311 | void Steer_Trans_Start() 312 | { 313 | steer_trans = STEER_MAX; 314 | Left_per_Delta = (Left_per - Left_per_Targ) / STEER_MAX; 315 | Right_per_Delta = (Right_per - Right_per_Targ) / STEER_MAX; 316 | } 317 | void Trans_Steer() { 318 | if (steer_trans > 0) 319 | { 320 | steer_trans--; 321 | Left_per -= Left_per_Delta; 322 | Right_per -= Right_per_Delta; 323 | Serial.print(steer_trans); 324 | Serial.print(" "); 325 | Serial.println(Left_per); 326 | Serial.print(" "); 327 | Serial.println(Right_per); 328 | } 329 | } 330 | 331 | void Trans() { 332 | if (transition > 0) 333 | { 334 | transition--; 335 | Stride_length -= Stride_length_Delta; 336 | Serial.print(transition); 337 | Serial.print(" "); 338 | Serial.println(Stride_length); 339 | } 340 | } 341 | 342 | void walk() 343 | { 344 | leg_phase[0] = (double) walk_counter; 345 | leg_phase[1] = (double)walk_counter + 0.5 * walk_cnt_max; 346 | leg_phase[2] = (double)walk_counter + 0.5 * walk_cnt_max; 347 | leg_phase[3] = (double)walk_counter - 0.0 * walk_cnt_max; 348 | int leg; 349 | for (leg = 0; leg < LC; leg++) 350 | { 351 | if (leg_phase[leg] > walk_cnt_max) 352 | { 353 | leg_phase[leg] -= walk_cnt_max; 354 | } 355 | else if (leg_phase[leg] < 0) 356 | { 357 | leg_phase[leg] += walk_cnt_max; 358 | } 359 | } 360 | 361 | Stride_length_a[0] = Stride_length*Left_per; 362 | Stride_length_a[1] = Stride_length*Right_per; 363 | Stride_length_a[2] = Stride_length*Left_per; 364 | Stride_length_a[3] = Stride_length*Right_per; 365 | 366 | 367 | 368 | double cycle; 369 | 370 | for (leg = 0; leg < LC; leg++) 371 | //leg = 0; 372 | { 373 | if ((leg_phase[leg] / (double)walk_cnt_max)< TOG ) 374 | { 375 | cycle = ((double)leg_phase[leg] / ((double)walk_cnt_max*(double)TOG)) * 3.1415; 376 | footx[leg] = (Stride_length_a[leg]*0.5 * cos(cycle)) + Stride_offset; 377 | footy[leg] = Base_Height - (Foot_Off_Ground * sin(cycle)); 378 | } 379 | else 380 | { 381 | cycle = (((double)walk_cnt_max-(double)leg_phase[leg]) / ((double)walk_cnt_max * (1.0-(double)TOG))); 382 | footx[leg] = (-Stride_length_a[leg] * (cycle-0.5)) + Stride_offset; 383 | footy[leg] = Base_Height; 384 | } 385 | 386 | footz[leg] = 40; 387 | } 388 | 389 | } 390 | int pwm_set = 100; 391 | void Send_Pos_CAN() 392 | { 393 | CAN_Send(20, 1, -(int)k_ang[0], pwm_set); 394 | delayMicroseconds(1); 395 | CAN_Send(21, 1, -(int)h_ang[0], pwm_set); 396 | delayMicroseconds(1); 397 | CAN_Send(10, 1, -(int)k_ang[1], pwm_set); 398 | delayMicroseconds(1); 399 | CAN_Send(11, 1, -(int)h_ang[1], pwm_set); 400 | delayMicroseconds(1); 401 | CAN_Send(40, 1, -(int)k_ang[2], pwm_set); 402 | delayMicroseconds(1); 403 | CAN_Send(41, 1, (int)h_ang[2], pwm_set); 404 | delayMicroseconds(1); 405 | CAN_Send(30, 1, -(int)k_ang[3], pwm_set); 406 | delayMicroseconds(1); 407 | CAN_Send(31, 1, (int)h_ang[3], pwm_set); 408 | } 409 | 410 | 411 | int mov=0; 412 | int tar; 413 | int cur; 414 | int sp=200; 415 | #define TILT 3000 416 | 417 | 418 | 419 | void IRAM_ATTR loop() { 420 | byte c; 421 | int leg_cnt; 422 | 423 | if (mov || transition || walk_counter ) 424 | { 425 | walk_cnt_max = sp; 426 | walk_counter++; 427 | if (walk_counter > walk_cnt_max) 428 | walk_counter = 0; 429 | 430 | Trans(); 431 | Trans_Steer(); 432 | walk(); 433 | 434 | for (leg_cnt = 0; leg_cnt150) 642 | Stride_length=150; 643 | SerialBT.print("Stride_length "); 644 | SerialBT.println(Stride_length); 645 | } 646 | else if (c=='l') 647 | { 648 | Stride_offset=Stride_offset-2; 649 | if (Stride_offset<-50) 650 | Stride_offset=-50; 651 | SerialBT.print("Stride_offset "); 652 | SerialBT.println(Stride_offset); 653 | } 654 | else if (c=='k') 655 | { 656 | Stride_offset=Stride_offset+2; 657 | if (Stride_offset>50) 658 | Stride_offset=50; 659 | SerialBT.print("Stride_offset "); 660 | SerialBT.println(Stride_offset); 661 | } 662 | else if (c=='j') 663 | { 664 | TOG=TOG-0.02; 665 | if (TOG<0) 666 | TOG=0; 667 | SerialBT.print("TOG "); 668 | SerialBT.println(TOG); 669 | } 670 | else if (c=='h') 671 | { 672 | TOG=TOG+.02; 673 | if (TOG>1) 674 | TOG=1; 675 | SerialBT.print("TOG "); 676 | SerialBT.println(TOG); 677 | } 678 | else if (c=='g') 679 | { 680 | Base_Height=Base_Height-5; 681 | if (Base_Height<100) 682 | Base_Height=100; 683 | SerialBT.print("Base_Height "); 684 | SerialBT.println(Base_Height); 685 | } 686 | else if (c=='f') 687 | { 688 | Base_Height=Base_Height+5; 689 | if (Base_Height>250) 690 | Base_Height=250; 691 | SerialBT.print("Base_Height "); 692 | SerialBT.println(Base_Height); 693 | } 694 | else if (c=='i') 695 | { 696 | Foot_Off_Ground=Foot_Off_Ground-5; 697 | if (Foot_Off_Ground<0) 698 | Foot_Off_Ground=0; 699 | SerialBT.print("Foot_Off_Ground "); 700 | SerialBT.println(Foot_Off_Ground); 701 | } 702 | else if (c=='u') 703 | { 704 | Foot_Off_Ground=Foot_Off_Ground+5; 705 | if (Foot_Off_Ground>100) 706 | Foot_Off_Ground=100; 707 | SerialBT.print("Foot_Off_Ground "); 708 | SerialBT.println(Foot_Off_Ground); 709 | } 710 | else if (c=='z') 711 | { 712 | Left_per_Targ = 0.5; 713 | Right_per_Targ = -0.5; 714 | Steer_Trans_Start(); 715 | SerialBT.println("left "); 716 | } 717 | else if (c=='x') 718 | { 719 | Left_per_Targ = 1.0; 720 | Right_per_Targ = 1.0; 721 | Steer_Trans_Start(); 722 | SerialBT.print("straight "); 723 | } 724 | else if (c=='c') 725 | { 726 | Left_per_Targ = -0.5; 727 | Right_per_Targ = 0.5; 728 | Steer_Trans_Start(); 729 | SerialBT.print("right "); 730 | } 731 | 732 | } 733 | } 734 | -------------------------------------------------------------------------------- /esp-now-had.ino: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2019 by Jacob Wachlin 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | // A minimum example of using esp-now for communication on ESP32 HW 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #define WIFI_CHANNEL (1) 29 | #define LED_PIN (2) 30 | 31 | static uint8_t broadcast_mac[] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; 32 | 33 | typedef struct __attribute__((packed)) esp_now_msg_t 34 | { 35 | uint32_t address; 36 | uint32_t counter; 37 | // Can put lots of things here... 38 | } esp_now_msg_t; 39 | 40 | static void handle_error(esp_err_t err) 41 | { 42 | switch (err) 43 | { 44 | case ESP_ERR_ESPNOW_NOT_INIT: 45 | Serial.println("Not init"); 46 | break; 47 | 48 | case ESP_ERR_ESPNOW_ARG: 49 | Serial.println("Argument invalid"); 50 | break; 51 | 52 | case ESP_ERR_ESPNOW_INTERNAL: 53 | Serial.println("Internal error"); 54 | break; 55 | 56 | case ESP_ERR_ESPNOW_NO_MEM: 57 | Serial.println("Out of memory"); 58 | break; 59 | 60 | case ESP_ERR_ESPNOW_NOT_FOUND: 61 | Serial.println("Peer is not found"); 62 | break; 63 | 64 | case ESP_ERR_ESPNOW_IF: 65 | Serial.println("Current WiFi interface doesn't match that of peer"); 66 | break; 67 | 68 | default: 69 | break; 70 | } 71 | } 72 | 73 | static void msg_recv_cb(const uint8_t *mac_addr, const uint8_t *data, int len) 74 | { 75 | //Serial.println("RX_MESSAGE"); 76 | if (len == sizeof(esp_now_msg_t)) 77 | { 78 | esp_now_msg_t msg; 79 | memcpy(&msg, data, len); 80 | 81 | //Serial.print("Counter: "); 82 | Serial.println(msg.counter); 83 | //digitalWrite(LED_PIN, !digitalRead(LED_PIN)); 84 | } 85 | } 86 | 87 | static void msg_send_cb(const uint8_t* mac, esp_now_send_status_t sendStatus) 88 | { 89 | 90 | switch (sendStatus) 91 | { 92 | case ESP_NOW_SEND_SUCCESS: 93 | Serial.println("S"); 94 | break; 95 | 96 | case ESP_NOW_SEND_FAIL: 97 | Serial.println("F"); 98 | break; 99 | 100 | default: 101 | break; 102 | } 103 | } 104 | 105 | static void send_msg(esp_now_msg_t * msg) 106 | { 107 | // Pack 108 | uint16_t packet_size = sizeof(esp_now_msg_t); 109 | uint8_t msg_data[packet_size]; 110 | memcpy(&msg_data[0], msg, sizeof(esp_now_msg_t)); 111 | 112 | esp_err_t status = esp_now_send(broadcast_mac, msg_data, packet_size); 113 | if (ESP_OK != status) 114 | { 115 | Serial.println("Error sending message"); 116 | handle_error(status); 117 | } 118 | } 119 | 120 | static void network_setup(void) 121 | { 122 | esp_wifi_internal_set_fix_rate(10); 123 | //Puts ESP in STATION MODE 124 | WiFi.mode(WIFI_STA); 125 | WiFi.disconnect(); 126 | 127 | if (esp_now_init() != 0) 128 | { 129 | Serial.println("esp_now_init Fail"); 130 | return; 131 | } 132 | 133 | esp_now_peer_info_t peer_info; 134 | peer_info.channel = WIFI_CHANNEL; 135 | memcpy(peer_info.peer_addr, broadcast_mac, 6); 136 | peer_info.ifidx = ESP_IF_WIFI_STA; 137 | peer_info.encrypt = false; 138 | esp_err_t status = esp_now_add_peer(&peer_info); 139 | if (ESP_OK != status) 140 | { 141 | Serial.println("Could not add peer"); 142 | handle_error(status); 143 | } 144 | 145 | // Set up callback 146 | status = esp_now_register_recv_cb(msg_recv_cb); 147 | if (ESP_OK != status) 148 | { 149 | Serial.println("Could not register callback"); 150 | handle_error(status); 151 | } 152 | 153 | status = esp_now_register_send_cb(msg_send_cb); 154 | if (ESP_OK != status) 155 | { 156 | Serial.println("Could not register send callback"); 157 | handle_error(status); 158 | } 159 | } 160 | 161 | void setup() { 162 | Serial.begin(115200); 163 | 164 | pinMode(LED_PIN,OUTPUT); 165 | digitalWrite(LED_PIN,LOW); 166 | 167 | network_setup(); 168 | } 169 | 170 | void loop() { 171 | // Need some delay for watchdog feeding in loop 172 | delay(2); 173 | 174 | static uint32_t counter = 0; 175 | esp_now_msg_t msg; 176 | msg.address = 0; 177 | msg.counter = ++counter; 178 | send_msg(&msg); 179 | 180 | 181 | } 182 | --------------------------------------------------------------------------------