KT motor controllers -- Flexible OpenSource firmware for BMSBattery S/Kunteng KT motor controllers (0.25kW up to 5kW)

stancecoke said:
casainho said:
I see it uses speed from the external sensor and I wonder if it can be avoided for the correct motor function, as you know the motor works on original firmware without that speed sensor.
This may work with a direct drive, but with a geared motor with a freewheel, you get no speedsignal if motor is not running while your bike is rolling. Serveral geared motors have an "extern" speedsensor internally already (white wire of the hall-sensor connector)
Ok and that should be optional. My S06x controllers do work without the external sensor.

stancecoke said:
I just updated the TORQUESENSOR_EXPERIMENTING branch with the 50 Hz control loop and 10 Hz communicaton loop. It works so far, Current and Speed is displayed correctly, Speed- and Current- Limitations work with P-control. This could be improved with a PI-control.

I think I will take a break at this point until somebody implements the display communication....
Ok, I will reuse your code. But you are not implementing "torque+speed" control and I think we should go with that because is like the original firmware does and seems the best method if we look at the discussion thread about the torque control methods.

Just understood looking at your code that torque sensor is like PAS+throttle :)
 
daffy99 said:
Is it possible to limit on the battery side, independently? E.g. "only extract at most x Amps, ever"? That would protect the battery pack (and its fuse, and BMS, and ...), and take care of battery pack aging. That also could be combined with ambient temperature, when available, again to protect the battery pack. LCD3 displays show temperature, but I am not sure which temperature, and https://endless-sphere.com/forums/viewtopic.php?t=73475 seems to indicate that this flows from the controller to the display, so it would be "something" temperature, if at all.

All this energy flow limiting would then offer a great, safe envelope in which it is possible to experiment with minimal risk to the hardware?

In case the firmware does not have battery low voltage protection yet, that would be another protective feature. I'd rather have control via firmware than depending on the battery BMS, in case there is one.

Again, for me "protection" and thus creating a safe zone to operate is the first step; after that, funky features could be added, such as overdrive mode or so :D
Yes, the max battery current is controlled by the motor controller. That max value should be a configuration and no more than what the controller holds. You can see my latest video where that current is controlled by the throttle.

The temperature that LCD shows are the one from the motor, when the motor have the temperature sensor wire output -- but I am not sure if BMSBattery S controllers have this hardware input.

Firmware does have low voltage protection, the code done by stancecoke:
Code:
  if (ui8_BatteryVoltage < BATTERY_VOLTAGE_MIN_VALUE) {
      ui32_setpoint=0; 	// highest priority: Stop motor for undervoltage protection
      TIM1_CtrlPWMOutputs(DISABLE);
            uint_PWM_Enable=0;
            ui32_setpoint=0; 	// highest priority: Stop motor for undervoltage protection
      printf("Low voltage! %d\n",ui8_BatteryVoltage);
  }
 
daffy99 said:
stancecoke said:
I think I will take a break at this point until somebody implements the display communication....
Anyway, I'd be happy to volunteer implementing all of this blind (the protocal seemed to be awfully trivial, to be honest). After all, we all know that once it compiles, it's done. Riiiiight?

I'll get started by getting at least some branch to compile locally.
the protocal seemed to be awfully trivial, to be honest
Isn't that great!? seems it should be quick and easy to implement, I really like that :) -- also means less resources needed and so cheaper hardware :)

Would be great if you could implement the LCD/bluetooth protocol. I have a spare LCD just for this development and so I will be able to test!
I think the branchs from stancecoke are the best to start since them seem to be the most complete. I need to finish the motor controller code to integrate on that branch.

I am seeing the firmware structure like the following diagram. I think that you can avoid for now the storage but you can also implement if you prefer - sould also be trivial :)

firmware_structure.png

Source file is here: https://github.com/OpenSource-EBike-firmware/BMSBattery_S_controllers_firmware/blob/master/tools/ -- firmware_structure*
 
I changed the code to start and read ADC values. ADC now reads in sequence the 10 channels and then stops (values are stored on ADC buffers). It should take about 18us to read all the 10 channels but happens in parallel, don't take an processing time -- it should be the most optimized way, even because it is in sync with start of PWM cycle that seems to be the best way to read ADC on motor controllers.
The start of read is triggered on the start of PWM cycle/interrupt and this way the ADCs will be read every PWM cycle with minimal code/processing time.

Trigger the start of read:
Code:
void TIM1_UPD_OVF_TRG_BRK_IRQHandler(void) __interrupt(TIM1_UPD_OVF_TRG_BRK_IRQHANDLER)
{
  // start ADC all channels, scan conversion (buffered)
  ADC1->CSR &= 0x09; // clear EOC flag first (selectd also channel 9)
  ADC1_StartConversion ();

And for reading 8 bits, which should be very fast:
Code:
uint8_t ui8_adc_read_throttle (void)
{
  return *(uint8_t*)(0x53E8);
}

And the 10 bits reading:
Code:
uint8_t ui16_adc_read_motor_total_current_filtered (void)
{
  uint16_t temph;
  uint8_t templ;

  templ = *(uint8_t*)(0x53F1);
  temph = *(uint8_t*)(0x53F0);

  return (uint16_t)((uint16_t)((uint16_t)templ << 6) | (uint16_t)(temph << 8));
}

I tested the trigger ADC read using TIM1/PWM Update Event but the only saved code would be "ADC1_StartConversion ();" and I think the EOC could happen only every 2 PWM cycles and so would be reducing/halve the ADC reading frequency...
 
casainho said:
My S06x controllers do work without the external sensor.
OK, but you will get no speed information if you are rolling down hill etc. You can configure it in the Kunteng Display (parameter P2) if you want to use the external sensor with the original firmware. If you don't want to use an external sensor, you have to set the right value to parameter P1 according to your ratio of erpm to wheel rpm to get the correct speed displayed with the original firmware.

casainho said:
Ok, I will reuse your code. But you are not implementing "torque+speed" control and I think we should go with that because is like the original firmware does and seems the best method if we look at the discussion thread about the torque control methods.
I tried this "torque+speed" mode with my bike with Lishui Controllers, of course it is better than simple speed mode but still quite stupid. I'm a fan of Torque-Sensor mode, that's the only way to have "bicycle-ride" feeling with an ebike.
Normaly the so called "torque-simulation" controllers ramp up the current with the cadence until the pre-defined "PAS-level" is reached.


casainho said:
Just understood looking at your code that torque sensor is like PAS+throttle :)
Yes, until the torque-signal is the same as the throttle signal in principle. But the setpoint is not just the throttle/torque value but calculated from factor x torque x cadence

casainho said:
stancecoke said:
casainho said:
I would like to have that files and images there
I've uploaded an .ods file to github.
I was looking for that file and it does not exist.
UUps. It was deleted with the old feature_torque_sensor branch.
I've uploaded it again to the TORQUESENSOR_EXPERIMENTING branch.

@daffy99: Nice to see you helping! Thumb up!

Regards
stancecoke
 
Last edited:
I tried to optimize this code, following some ideas of NJay, mainly using a table with values centered on zero and not on 127:

Code:
void pwm_apply_duty_cycle (uint8_t ui8_duty_cycle_value)
{
  static uint8_t ui8__duty_cycle;
  static uint8_t ui8_temp;

  ui8__duty_cycle = ui8_duty_cycle_value;

  // scale and apply _duty_cycle
  ui8_temp = ui8_svm_table[ui8_motor_rotor_position];
  if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)
  {
    ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle;
    ui8_temp = (uint8_t) (ui16_value >> 8);
    ui8_value_a = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp;
  }
  else
  {
    ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle;
    ui8_temp = (uint8_t) (ui16_value >> 8);
    ui8_value_a = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp;
  }

  // add 120 degrees and limit
  ui8_temp = ui8_svm_table[(uint8_t) (ui8_motor_rotor_position + 85 /* 120º */)];
  if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)
  {
    ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle;
    ui8_temp = (uint8_t) (ui16_value >> 8);
    ui8_value_b = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp;
  }
  else
  {
    ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle;
    ui8_temp = (uint8_t) (ui16_value >> 8);
    ui8_value_b = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp;
  }

  // subtract 120 degrees and limit
  ui8_temp = ui8_svm_table[(uint8_t) (ui8_motor_rotor_position + 171 /* 240º */)];
  if (ui8_temp > MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)
  {
    ui16_value = ((uint16_t) (ui8_temp - MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX)) * ui8__duty_cycle;
    ui8_temp = (uint8_t) (ui16_value >> 8);
    ui8_value_c = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX + ui8_temp;
  }
  else
  {
    ui16_value = ((uint16_t) (MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp)) * ui8__duty_cycle;
    ui8_temp = (uint8_t) (ui16_value >> 8);
    ui8_value_c = MIDDLE_PWM_VALUE_DUTY_CYCLE_MAX - ui8_temp;
  }

  // set final duty_cycle value
  TIM1_SetCompare1((uint16_t) (ui8_value_a << 1));
  TIM1_SetCompare2((uint16_t) (ui8_value_c << 1));
  TIM1_SetCompare3((uint16_t) (ui8_value_b << 1));
}

Code:
void pwm_apply_duty_cycle (uint8_t ui8_duty_cycle_value)
{
  int16_t i16_temp;

  // scale table values with duty_cycle value factor
  i16_temp = i8_svm_table [ui8_motor_rotor_position] * ui8_duty_cycle_value;
  ui16_value_a = i16_temp + 32512;
  ui16_value_a = ui16_value_a >> 7;

  i16_temp = i8_svm_table [(uint8_t) (ui8_motor_rotor_position + 85 /* 120º */)] * ui8_duty_cycle_value;
  ui16_value_b = i16_temp + 32512;
  ui16_value_b = ui16_value_b >> 7;

  i16_temp = i8_svm_table [(uint8_t) (ui8_motor_rotor_position + 171 /* 240º */)] * ui8_duty_cycle_value;
  ui16_value_c = i16_temp + 32512;
  ui16_value_c = ui16_value_c >> 7;

  // apply duty_cycle value
  TIM1_SetCompare1(ui16_value_a);
  TIM1_SetCompare2(ui16_value_c);
  TIM1_SetCompare3(ui16_value_b);
}

The optimized code seems short but it uses more processing time!! :-(
PWM cycle processing time:
- old code: 42.4us
- new optimized code: 47.2
So it takes +4.8us!! Maybe the math with signed ints takes more processing time... and even the code seems to have some bugs because motor works but with some problems... time to go back to old code :)
 
casainho said:
My plan for next week: OSEC firmware v0.1
- develop the firmware part for the motor over current:
--using this information: https://opensourceebikefirmware.bitbucket.io/Various--Endless-sphere.com_forum_messages--2017.09.14_-_measuring_motor_current.html

- develop the firmware part to read motor current (Id current)
-- read the phase B current value at 90º:when ui16_PWM_cycles_counter == (ui16_PWM_cycles_counter_total >> 2)

- decide for a sinewave form
-- original is ok for read motor current but the other "advanced" shape seems better to give motor more speed and more silent. I will need to see I can read ok the motor current and if so, stick with this sinewave form

- close the motor controller firmware, this time will not have Regen

- try to put throttle controlling the motor torque/Id current instead of current PWM duty_cycle
-----

After having OSEC firmware v0.1, maybe some developers could focus on support the LCD whiles others (like me) could focus on motor control/throttle "torque-speed" and regen. Or maybe LCD could be a priority, because new users will prefer to have a full working system instead of an optimized system. Full working system would be motor working ok, with throttle or/and PAS and LCD.
Stancecoke, what do you think?
I got more or less what I wanted this week. The motor seems to work well (my both 2 different motors). But I need to test more and more the motor, like trying to block it and see if it maintains the torque. Also I can start reading more about regen, because on current firmware everytime the throttle decelerates, regen happen -- maybe I just need to limit the regen current... the same current I am limiting to power the motor, maybe I just need to limit the negative value...

Also nice that we have a new developer that wants to start with the communictions/LCD/bluetooth :)

And for some reason, 1 mosfet did burn and I had to exchange for other from my spare S06S controller.
 
casainho said:
I got more or less what I wanted this week
Yes, I think we have made good progress. From my point of view, we can start with tests on a real bike at this stage. Perhaps for safety reasons we should add the interrupt on OC pin.

casainho said:
Also I can start reading more about regen, because on current firmware everytime the throttle decelerates, regen happen
Do you read negative currents on ADC8 (values smaller than 312 @ 10bit) ? I never looked at the current value while releasing the throttle. I've implemented a feature that sets the phases floating if you release the throttle rapidly, but this is not really satisfying yet.
Anyway, I think regen is not that important, as estimated more than 95% of ebikers use motors (hub or middrive) with freewheels and therefore can't use regen, even if they want to....

Regards
stancecoke
 
casainho said:
regen happen

How can regen happen on a geared hub motor with a cassette freehub? (Showing my ignorance here, sorry)

casainho said:
we have a new developer

Hold your horses, please, and please do manage your expectations. While I am able to distinguish a bit from a byte, I have never, ever done any embedded system programming. Case in point: Searching some random STM8 docs, I did not find any occurrences for the character patterns "atomic", "volatile", "barrier", "cache", or "coherency". At least I now know the difference between the Harvard architecture and a von Neumann architecture. Well, as far as Wikipedia is concerned :D

I also have yet to receive the full set of components required to build up an electrically assisted bicycle (motor and battery are "in transit" at the moment), and only after having obtained some components I'd be happy to kill, would I be able to contribute in any meaningful fashion. After all, my main goal is to - ride. :)
 
casainho said:
I got more or less what I wanted this week. The motor seems to work well (my both 2 different motors). But I need to test more and more the motor, like trying to block it and see if it maintains the torque.
I went to test blocking the motor with my hand while the motor max current were defined to 1A and 2.3A. I configured the duty_cycle to ramp up/down at ui8_duty_cycle_ramp_inverse_step = 100; multiples of every PWM cycle, this way the acceleration/deceleration is a bit slower because I wanted to make the tests with my hand blocking but in a safe way.

The results are as I expected, as can be seen on the videos, I could block the motor and it immediately jumps to the motor max current and keep doing the torque/force in my hand!! Stancecoke, this is what I was looking for when doing current control so fast and not slow on main loop -- I think the results are great.

There is one thing that is not always good yet, sometimes it looses torque/force at some positions and that also happens when starting the motor. Maybe I need to adjust a bit #define MOTOR_ROTOR_DELTA_PHASE_ANGLE_RIGHT 235 or if needed, implement the 6 steps as I did before (uses more memory).

This same logic can now be applied to motor over current, that will be an higher value, near the controller max current limit. What should I do, disable the motor until user resets the system or do the same control, reducing duty_cycle??

[youtube]mLQpM0-V_qk[/youtube]

[youtube]n8ZxFfntZpI[/youtube]
 
casainho said:
I tried to optimize this code, following some ideas of NJay, mainly using a table with values centered on zero and not on 127:
(...)
The optimized code seems short but it uses more processing time!! :-(
PWM cycle processing time:
- old code: 42.4us
- new optimized code: 47.2
So it takes +4.8us!! Maybe the math with signed ints takes more processing time... and even the code seems to have some bugs because motor works but with some problems... time to go back to old code :)
Time to debug the new code :D . Do you have the .LST files (from old code and new code)? They have the assembly generated from the C code, and that's what I use to see what the compiler is doing and adjust accordingly.

The results are as I expected, as can be seen on the videos, I could block the motor and it immediately jumps to the motor max current and keep doing the torque/force in my hand!! Stancecoke, this is what I was looking for when doing current control so fast and not slow on main loop -- I think the results are great.
So you're limiting the battery current? What's the motor current for that 1A battery current?
 
Njay said:
So you're limiting the battery current? What's the motor current for that 1A battery current?
Using FOC, I assume battery current ~= motor current. But FOC only runs at a minimum speed and not when motor is blocked, where it runs kind of 6 steps... I don't know how to measure motor current, maybe it is not possible on this hardware and I would say both currents are similar.
I prefer to call it motor current because it makes clear when reading the firmware:
1. set_motor_max_current ();
2. set_battery_max_current ();

On the motor module interface, I think is clear 1. and not 2., even if it is an imprecision.
 
stancecoke said:
I tried this "torque+speed" mode with my bike with Lishui Controllers, of course it is better than simple speed mode but still quite stupid. I'm a fan of Torque-Sensor mode, that's the only way to have "bicycle-ride" feeling with an ebike.
If wasn't you, I would not be interested in using the torque sensor because they seem expensive and I the chinese cheap ones lack documentation -- I hope with this firmware to also document a few different user cases, so others can learn and understand how to use all this to build their specific ebikes.

stancecoke said:
Yes, until the torque-signal is the same as the throttle signal in principle. But the setpoint is not just the throttle/torque value but calculated from factor x torque x cadence
Code:
ui32_setpoint=(fummelfaktor*sumtorque)/PAS;
Now we can define motor torque!! Wouldn't make sense to map the torque from the torque sensor to the motor torque?? And let user choose is cadence :) -- eventually, when no pedaling, limit the motor torque to like 25% or create a ramp torque...
What do you think?

For my 7 years old son, I updated his bike with an electric motor and put a throttle -- he likes a lot to build vehicles with legos and I wanted him to have his first vehicle, electric, where he could control the motor with a throttle to feel how it is to control and drive a motor vehicle! (I recorded a video on the first time!!). Later I changed to a PAS and he didn't like much but found that we could cheat the PAS by keeping the cadence but not making force :) -- now I would like to add the torque sensor - I will buy 2 units from BMSBattery, one for development and other for his bike.

[youtube]OW73Gx0u54g[/youtube]
 
These guys sell Kunteng controllers for real cheap. https://www.aliexpress.com/store/group/Controller-and-Set/706709_259085738.html?spm=2114.12010612.0.0.2fe64d22PjPOb9
 
7bigjohn said:
These guys sell Kunteng controllers for real cheap. https://www.aliexpress.com/store/group/Controller-and-Set/706709_259085738.html?spm=2114.12010612.0.0.2fe64d22PjPOb9

Lowest total price including shipping that I have been able to find is through pswpower (https://www.aliexpress.com/store/group/Sine-Wave-controller/1314442_506798312.html)

FWIW, while USD 23 is low as far as retail is concerned, the profit margin on any one of these controllers is probably around 200% (product value of USD 7 declared at customs, which seems to be not off).
 
casainho said:
Now we can define motor torque!! Wouldn't make sense to map the torque from the torque sensor to the motor torque?? And let user choose is cadence
The intention is to make the motor power proportional to the human power.
For example, if the rider pedals with 100w, he gets additional (factor 1) 100w from the motor on top. The factor can be set by the user over the display or a simple pushbutton for example.

In rotational systems, power is the product of the torque τ and angular velocity ω,

P ( t ) = τ ⋅ ω
wikipedia.

In principle, we need two factors, one to calculate the physical correct human power in watts (from the timetics of PAScounter and the adc value of torque)and then the assistance level factor that is set by the user. From this we can set the electric power for the motor P=U*I

regards
stancecoke
 
stancecoke said:
casainho said:
Now we can define motor torque!! Wouldn't make sense to map the torque from the torque sensor to the motor torque?? And let user choose is cadence
The intention is to make the motor power proportional to the human power.
For example, if the rider pedals with 100w, he gets additional (factor 1) 100w from the motor on top. The factor can be set by the user over the display or a simple pushbutton for example.
Right.

stancecoke said:
In rotational systems, power is the product of the torque τ and angular velocity ω,

P ( t ) = τ ⋅ ω
wikipedia.
Code:
ui32_setpoint=(fummelfaktor*sumtorque)/PAS;
Ok now I understand. I thought sumtorque was already the torque value and with this the user would get less motor torque with the increase of cadence.

So I see (sumtorque/PAS) as an average or integral. If user pedals 1 impulse per second -> PAS = 1; max of torque pulse / 1 = max of torque pulse. If pedals 2 pulses per second, PAS = 2; (max of torque pulse * 2) / 2 = max of torque pulse -- assuming max of torque pulse equal in the 2 pulses.

I will adopt your code to my branch, to be the torque/PAS/torque sensor controller and so maybe after I can merge to master branch. I want to add throttle mode "torque+speed":
firmware_structure.png
 
casainho said:
So I see (sumtorque/PAS) as an average or integral. If user pedals 1 impulse per second -> PAS = 1; max of torque pulse / 1 = max of torque pulse. If pedals 2 pulses per second, PAS = 2; (max of torque pulse * 2) / 2 = max of torque pulse -- assuming max of torque pulse equal in the 2 pulses.

I think I have to explain this in detail.
You get several PAS Pulses with one crank revolution. How many pulses you get depends on your system. With a normal PAS disk from the common corversion kits you can easily count the number of magnets that are placed in the disk (normally something about 8 till 12 magnets). With the various Torque-Sensors, there are much more PAS pulses e.g. 16 or even 32 pulses per crank revolution.
In my code the number of PAS-Pulses per crank revoltion is defined in the main.h

Code:
#define NUMBER_OF_PAS_MAGS 16 //16 for sensor from BMSBattery, 32 for Sempu-Sensor

With each PAS-pulse detected (by the PAS-Pin Interrupt), the torque at the crank is read in (in a single AD-conversion) and stored into an array that has the length of "NUMBER_OF_PAS_MAGS"
Code:
...
      ui8_temp = adc_read_throttle (); //read in recent torque value
      ...
ui16_torque[ui8_torque_index]= (uint8_t) map (ui8_temp , ADC_THROTTLE_MIN_VALUE, ADC_THROTTLE_MAX_VALUE, 0, SETPOINT_MAX_VALUE);

then all values in the array are added to get the value sumtorque, that stands for the average torque at the crank within one crank revolution.

Code:
ui16_sum_torque = 0;
      for(a = 0; a < NUMBER_OF_PAS_MAGS; a++) {			// sum up array content
	   ui16_sum_torque+= ui16_torque[a];
}

The value "PAS" is the duration between two PAS-Pulses. The faster you turn the crank, the lower PAS gets. So the cadence is proportional to 1/PAS.
The "PAScounter" is incremented each fast loop run (15,625 kHz). With each PAS interrupt the PAS value is set to the PASCounter value und the PAS Counter gets resetted.

Code:
if (ui8_PAS_Flag == 1) //ui8_PAS_Flag is set by the PAS-Pin interrupt routine
    {
      ui16_PAS=ui16_PAS_Counter; 		//save recent cadence
      
      ui16_PAS_Counter=0;			//reset PAS Counter

ui8_PAS_Flag =0; //reset interrupt flag

So you have the information for an average torque at the crank and the information of the duration between two PAS-Pulses and you can calculate the already known function
Code:
ui32_setpoint=(fummelfaktor*sumtorque)/PAS; //calculate setpoint

as you can read from the word "fummelfaktor" this factor is not physically derived but set by trial and error, yet :wink: .

Regards
stancecoke
 
stancecoke said:
casainho said:
So I see (sumtorque/PAS) as an average or integral. If user pedals 1 impulse per second -> PAS = 1; max of torque pulse / 1 = max of torque pulse. If pedals 2 pulses per second, PAS = 2; (max of torque pulse * 2) / 2 = max of torque pulse -- assuming max of torque pulse equal in the 2 pulses.

I think I have to explain this in detail.
You get several PAS Pulses with one crank revolution. How many pulses you get depends on your system. With a normal PAS disk from the common corversion kits you can easily count the number of magnets that are placed in the disk (normally something about 8 till 12 magnets). With the various Torque-Sensors, there are much more PAS pulses e.g. 16 or even 32 pulses per crank revolution.
In my code the number of PAS-Pulses per crank revoltion is defined in the main.h

Code:
#define NUMBER_OF_PAS_MAGS 16 //16 for sensor from BMSBattery, 32 for Sempu-Sensor

With each PAS-pulse detected (by the PAS-Pin Interrupt), the torque at the crank is read in (in a single AD-conversion) and stored into an array that has the length of "NUMBER_OF_PAS_MAGS"
Code:
...
      ui8_temp = adc_read_throttle (); //read in recent torque value
      ...
ui16_torque[ui8_torque_index]= (uint8_t) map (ui8_temp , ADC_THROTTLE_MIN_VALUE, ADC_THROTTLE_MAX_VALUE, 0, SETPOINT_MAX_VALUE);

then all values in the array are added to get the value sumtorque, that stands for the average torque at the crank within one crank revolution.

Code:
ui16_sum_torque = 0;
      for(a = 0; a < NUMBER_OF_PAS_MAGS; a++) {			// sum up array content
	   ui16_sum_torque+= ui16_torque[a];
}

The value "PAS" is the duration between two PAS-Pulses. The faster you turn the crank, the lower PAS gets. So the cadence is proportional to 1/PAS.
The "PAScounter" is incremented each fast loop run (15,625 kHz). With each PAS interrupt the PAS value is set to the PASCounter value und the PAS Counter gets resetted.

Code:
if (ui8_PAS_Flag == 1) //ui8_PAS_Flag is set by the PAS-Pin interrupt routine
    {
      ui16_PAS=ui16_PAS_Counter; 		//save recent cadence
      
      ui16_PAS_Counter=0;			//reset PAS Counter

ui8_PAS_Flag =0; //reset interrupt flag

So you have the information for an average torque at the crank and the information of the duration between two PAS-Pulses and you can calculate the already known function
Code:
ui32_setpoint=(fummelfaktor*sumtorque)/PAS; //calculate setpoint

as you can read from the word "fummelfaktor" this factor is not physically derived but set by trial and error, yet :wink: .
Perfect!! Took me sometime to do the math in my head and learn how it works when reducing a gear to pedal faster but lowering the torque -- I must say I don not ride bicycle so often nor I am a pro :)

One question:
Code:
ui32_setpoint=(fummelfaktor*sumtorque)/PAS; //calculate setpoint
At stop, PAS is very big and so ui32_setpoint will be zero, right? on this situation we may use directly torque so we can get the help of the motor for startup, right?

About the motor, I am not having any problem when starting from 0 throttle while the motor is rotating already but maybe because there is very small kynetic energy in the motor in my experiments... maybe with a rider on the bicycle this will be a problem... I need to think on this!!! (before start the testes on a real bicycle).
 
casainho said:
One question:
Code:
ui32_setpoint=(fummelfaktor*sumtorque)/PAS; //calculate setpoint
At stop, PAS is very big and so ui32_setpoint will be zero, right? on this situation we may use directly torque so we can get the help of the motor for startup, right?
Yes, this is the way it is solved in the code of the "Forumscontroller" from the German forum.

Code:
#ifdef SUPPORT_XCELL_RT
    power_poti = poti_stat/102300.0* curr_power_poti_max*power_human*(1+spd/20.0); //power_poti_max is in this control mode interpreted as percentage. Example: power_poti_max=200 means; motor power = 200% of human power
#ifdef SUPPORT_TORQUE_THROTTLE                              //we want to trigger throttle just by pedal torque
    if (abs(torque_instant)>torque_throttle_min)            //we are above the threshold to trigger throttle
    {
      double power_torque_throttle = abs(torque_instant/torque_throttle_full*poti_stat/1023*curr_power_max);  //translate torque_throttle_full to maximum power
      power_throttle = max(power_throttle,power_torque_throttle); //decide if thumb throttle or torque throttle are higher
      power_throttle = constrain(power_throttle,0,curr_power_max); //constrain throttle value to maximum power 
    }
#endif
#endif
...
if ((power_throttle) > (power_poti))                     //IF power set by throttle IS GREATER THAN power set by poti (throttle override)
    {
        myPID.SetTunings(pid_p_throttle,pid_i_throttle,0);   //THEN throttle mode: throttle sets power with "agressive" p and i parameters        power_set=throttle_stat/1023.0*power_max;
        power_set = power_throttle;
    }
    else                                                     //ELSE poti mode: poti sets power with "soft" p and i parameters
    {
        myPID.SetTunings(pid_p,pid_i,0);
        power_set = power_poti;
    }
In this way you get assitance at starting from zero speed, but it leads to oszillating motor power (with each force put on the crank) until the human power is stable bigger than the simple torque value...

There is a little difference to our code, as "thumb throttle" and torque from Bottom Bracket Sensor are connected to different input pins. In this way you can override the Torque-Sensor with the thumb throttle.

Regards
stancecoke
 
Today I was trying to prepare the motor and throttle code, for the situation when we decelerate. Right now, when we do it, it does regen but I don't want to deal/implement regen right now. The idea is that when decelerating, the motor coast. And while it coast, I may want to accelerate again increasing the throttle... and on that time I need to know the correct voltages to apply on the motor terminals -- voltage amplitude and phase should equal to the same as generated by the motor when coasting, otherwise will be a short circuit!! The generated voltage is the BEMF.

To know the amplitude of BEMF, we can use the motor speed and multiply by the KV, so we will know how many volts are in the terminals. We need to apply and equal voltage to the terminals, so we know the battery voltage because we can read it and we multiply by the duty_cycle factor... in the end the code is this:

Code:
      // calc BEMF
      ui16_motor_BEMF = ui16_motor_speed_erps * MOTOR_KVOLTS_PER_ERPS;

      // CALC MIN DUTY_CYCLE
      // min duty_cycle = (BEMF * max duty_cycle) / Vbat
      // since BEMF is already multiplied by 255 (max duty_cycle), duty = BEMF / Vbat
      ui16_duty_cycle_min = ui16_motor_BEMF / ((ui8_adc_read_battery_voltage () * ADC_BATTERY_VOLTAGE_K) >> 8);

Then I detect when the new throttle value is lower and coast the motor. And I just enable the motor again when the throttle duty_cycle is equal or higher than the duty_cycle min (duty_cycle equivalent to the motor current BEMF):

Code:
      if (ui8_duty_cycle_target < ui8_duty_cycle_target_old)
      {
	motor_set_mode_coast ();
      }
      else
      {
	if (ui8_duty_cycle_target > ui16_duty_cycle_min)
	{
	  pwm_set_duty_cycle (ui8_duty_cycle_target);
	  motor_set_mode_run ();
	}
      }
      ui8_duty_cycle_target_old = ui8_duty_cycle_target;

It is bit hard to debug this in real time but seems to me that this part is working... but the all thing is failing and I think is because of the out of phase/lack of synchronization. FOC does the synchronization, but only when there is phase B current and when I coast the motor, there is no phase B current... and I think I am seeing this on the oscilloscope.
I need to think more on this thing!!!

Calculated BEMF on bottom and speed ERPs on top. The power supply had 30V. We can see at max speed that calculated BEMF is 30V as expected. At the end I did coast the motor and we can see a slow ramp decreasing the speed and calculated BEMF:
Code:
ui16_motor_BEMF = ui16_motor_speed_erps * MOTOR_KVOLTS_PER_ERPS;


Calculated min duty_cycle on bottom and speed ERPs on top. At the end I did coast the motor and we can see a slow ramp decreasing the min duty_cycle, as expected:
Code:
      ui16_duty_cycle_min = ui16_motor_BEMF / ((ui8_adc_read_battery_voltage () * ADC_BATTERY_VOLTAGE_K) >> 8);
 
The controllers from aliexpress arrived today. On one hand I'm a little sad, they are no Kunteng KT-36. On the other hand, I'm quite happy. They seem to be a new version of the well know "programmable" EB3xx controllers.

The processor is labeld with to lines:
079F9211
1635EB40x

I found no datasheet for this in the internet.... just a hint to 079F9211 on a russian forum.

I'll search for some more information and will try the "programming"

Regards
stancecoke
 
I got a resume of what is needed (from VESC) to do to drive the motor when it is running un-driven: http://vedder.se/2014/08/startup-torque-on-sensorless-bldc-motors/
When the motor is un-driven, the back-emf on all phases is analysed to track the position, direction and speed of the motor. So when the motor is already spinning, the algorithm will begin in the exact state of the motor (position, duty-cycle, direction).
The code is HUGE, and this part runs on every PWM cycle (and there is more code running before and after this one. This parts seems to be the relevant part. HOW TO DO IN OUR CASE WITH THE 16MHZ 8 bits STM8??
Code:
	if (m_state == MC_STATE_RUNNING) {
		
	} else {
		// Track back emf
		float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
		float Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
		float Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);

		// Clarke transform
		m_motor_state.v_alpha = (2.0 / 3.0) * Va - (1.0 / 3.0) * Vb - (1.0 / 3.0) * Vc;
		m_motor_state.v_beta = ONE_BY_SQRT3 * Vb - ONE_BY_SQRT3 * Vc;

		// Park transform
		float c,s;
		utils_fast_sincos_better(m_motor_state.phase, &s, &c);
		m_motor_state.vd = c * m_motor_state.v_alpha + s * m_motor_state.v_beta;
		m_motor_state.vq = c * m_motor_state.v_beta  - s * m_motor_state.v_alpha;

		// Update corresponding modulation
		m_motor_state.mod_d = m_motor_state.vd / ((2.0 / 3.0) * m_motor_state.v_bus);
		m_motor_state.mod_q = m_motor_state.vq / ((2.0 / 3.0) * m_motor_state.v_bus);

		// The current is 0 when the motor is undriven
		m_motor_state.i_alpha = 0.0;
		m_motor_state.i_beta = 0.0;
		m_motor_state.id = 0.0;
		m_motor_state.iq = 0.0;
		m_motor_state.id_filter = 0.0;
		m_motor_state.iq_filter = 0.0;
		m_motor_state.i_bus = 0.0;
		m_motor_state.i_abs = 0.0;
		m_motor_state.i_abs_filter = 0.0;

		// Run observer
		observer_update(m_motor_state.v_alpha, m_motor_state.v_beta,
				m_motor_state.i_alpha, m_motor_state.i_beta, dt, &m_observer_x1,
				&m_observer_x2, &m_phase_now_observer);

		switch (m_conf->foc_sensor_mode) {
		case FOC_SENSOR_MODE_ENCODER:
			m_motor_state.phase = correct_encoder(m_phase_now_observer, m_phase_now_encoder, m_pll_speed);
			break;
		case FOC_SENSOR_MODE_HALL:
			m_phase_now_observer = correct_hall(m_phase_now_observer, m_pll_speed, dt);
			m_motor_state.phase = m_phase_now_observer;
			break;
		case FOC_SENSOR_MODE_SENSORLESS:
			m_motor_state.phase = m_phase_now_observer;
			break;
		}
	}

	// Calculate duty cycle
	m_motor_state.duty_now = SIGN(m_motor_state.vq) *
			sqrtf(m_motor_state.mod_d * m_motor_state.mod_d +
					m_motor_state.mod_q * m_motor_state.mod_q) / SQRT3_BY_2;

	// Run PLL for speed estimation
	pll_run(m_motor_state.phase, dt, &m_pll_phase, &m_pll_speed);

	// Update tachometer
	static float phase_last = 0.0;
	int diff = (int)((utils_angle_difference_rad(m_motor_state.phase, phase_last) * (180.0 / M_PI)) / 60.0);
	if (diff != 0) {
		m_tachometer += diff;
		m_tachometer_abs += abs(diff);
		phase_last = m_motor_state.phase;
	}

	// Track position control angle
	// TODO: Have another look at this.
	float angle_now = 0.0;
	if (encoder_is_configured()) {
		angle_now = enc_ang;
	} else {
		angle_now = m_motor_state.phase * (180.0 / M_PI);
	}

	if (m_conf->p_pid_ang_div > 0.98 && m_conf->p_pid_ang_div < 1.02) {
		m_pos_pid_now = angle_now;
	} else {
		static float angle_last = 0.0;
		float diff_f = utils_angle_difference(angle_now, angle_last);
		angle_last = angle_now;
		m_pos_pid_now += diff_f / m_conf->p_pid_ang_div;
		utils_norm_angle((float*)&m_pos_pid_now);
	}

	// Run position control
	if (m_state == MC_STATE_RUNNING) {
		run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
	}

	// MCIF handler
	mc_interface_mc_timer_isr();

	last_inj_adc_isr_duration = (float) TIM12->CNT / 10000000.0;
}
 
daffy99 said:
casainho said:
regen happen
How can regen happen on a geared hub motor with a cassette freehub? (Showing my ignorance here, sorry)
Today I verified it does not regen and seems to work well with current firmware. I would like to know in detail how this geared motors clutch works... because when starting when the motor is un-driven and rotating, it just works and I would like to know how.

I think we I will try to go with a first version of working firmware only for this geared motors. For direct drive motors, I improved the firmware (I had a bug) but still makes noise etc - I have one more idea to improve...

Also I verified that battery/motor current is negative when regen happens and postive otherwise so to limit regen is just a matter to control that current by increasing duty_cyle when the regen current is higher than the limit -- the same happens but in contrary for motor working current, motor current is positive and to limit it we need to lower duty_cycle. Shane Colton just do this :)

daffy99 said:
casainho said:
we have a new developer
Hold your horses, please, and please do manage your expectations.
I would like to give you write access to github project for when you need for your development - please five me user id.
 
casainho said:
I would like to give you write access to github project for when you need for your development - please five me user id.

Thank you for the offer! My preferred approach is to use pull requests - eventually ;)

The good news is that I finally have a pedelec kit, the critical open challenge being that the custom battery pack seems to enjoy some sight-seeing by virtue of the German shipping company dropping a ball or two. :evil:. (Do not ever, I mean never-ever, use Germany company Hermes for getting anything shipped. Ever.)

The target controller would be a KT36ZWSR-EP01 once I have established that the original kit works with that original controller, and that the KT36/48SVPRD which I got from Aliexpress does everything better. Or I simply order another KT36/48SVPRD via Aliexpress. Or I stop rambling ;)

But I really need power. :cry:
 
Back
Top