#include <stdio.h>
#include <stdint.h>
 
#define MOTORVEL_HISTORY_LENGTH (40)
static int position;
static int motorVelIndex;
static int motorVel;
static int motorVelHistory[MOTORVEL_HISTORY_LENGTH];
static void PID_RunUpdateMotorVel(void)
{
/*
 * Also, since the trajectory generation has big variations, we will
 * feedback the motor velocity directly here.
 *
 * The maximum speed we go should fit in 16 bits, so overflows should
 * not matter here.
 *
 * We make this selectable, since the low pass in the current PI loop
 * may take care of this for us.  We may have to implement an error
 * dithering to account for range, will test to find out.
 */
    const int32_t motorpos = position; /* Cache the volatile */
    if (motorVelIndex >= MOTORVEL_HISTORY_LENGTH - 1)
    {
        motorVelIndex = 0;
    }
    else
    {
        motorVelIndex++;
    }
    motorVel = motorpos - motorVelHistory[motorVelIndex];
 
    motorVelHistory[motorVelIndex] = motorpos;
}
 
int main(void) {
	// your code goes here
	for (int i = 0; i < MOTORVEL_HISTORY_LENGTH + 2; i++)
	{
		position = i;
		PID_RunUpdateMotorVel();
		printf("Motor Vel=%d\n", motorVel);
	}
	return 0;
}
 

I2luY2x1ZGUgPHN0ZGlvLmg+CiNpbmNsdWRlIDxzdGRpbnQuaD4KCiNkZWZpbmUgTU9UT1JWRUxfSElTVE9SWV9MRU5HVEggKDQwKQpzdGF0aWMgaW50IHBvc2l0aW9uOwpzdGF0aWMgaW50IG1vdG9yVmVsSW5kZXg7CnN0YXRpYyBpbnQgbW90b3JWZWw7CnN0YXRpYyBpbnQgbW90b3JWZWxIaXN0b3J5W01PVE9SVkVMX0hJU1RPUllfTEVOR1RIXTsKc3RhdGljIHZvaWQgUElEX1J1blVwZGF0ZU1vdG9yVmVsKHZvaWQpCnsKLyoKICogQWxzbywgc2luY2UgdGhlIHRyYWplY3RvcnkgZ2VuZXJhdGlvbiBoYXMgYmlnIHZhcmlhdGlvbnMsIHdlIHdpbGwKICogZmVlZGJhY2sgdGhlIG1vdG9yIHZlbG9jaXR5IGRpcmVjdGx5IGhlcmUuCiAqCiAqIFRoZSBtYXhpbXVtIHNwZWVkIHdlIGdvIHNob3VsZCBmaXQgaW4gMTYgYml0cywgc28gb3ZlcmZsb3dzIHNob3VsZAogKiBub3QgbWF0dGVyIGhlcmUuCiAqCiAqIFdlIG1ha2UgdGhpcyBzZWxlY3RhYmxlLCBzaW5jZSB0aGUgbG93IHBhc3MgaW4gdGhlIGN1cnJlbnQgUEkgbG9vcAogKiBtYXkgdGFrZSBjYXJlIG9mIHRoaXMgZm9yIHVzLiAgV2UgbWF5IGhhdmUgdG8gaW1wbGVtZW50IGFuIGVycm9yCiAqIGRpdGhlcmluZyB0byBhY2NvdW50IGZvciByYW5nZSwgd2lsbCB0ZXN0IHRvIGZpbmQgb3V0LgogKi8KICAgIGNvbnN0IGludDMyX3QgbW90b3Jwb3MgPSBwb3NpdGlvbjsgLyogQ2FjaGUgdGhlIHZvbGF0aWxlICovCiAgICBpZiAobW90b3JWZWxJbmRleCA+PSBNT1RPUlZFTF9ISVNUT1JZX0xFTkdUSCAtIDEpCiAgICB7CiAgICAgICAgbW90b3JWZWxJbmRleCA9IDA7CiAgICB9CiAgICBlbHNlCiAgICB7CiAgICAgICAgbW90b3JWZWxJbmRleCsrOwogICAgfQogICAgbW90b3JWZWwgPSBtb3RvcnBvcyAtIG1vdG9yVmVsSGlzdG9yeVttb3RvclZlbEluZGV4XTsKCiAgICBtb3RvclZlbEhpc3RvcnlbbW90b3JWZWxJbmRleF0gPSBtb3RvcnBvczsKfQoKaW50IG1haW4odm9pZCkgewoJLy8geW91ciBjb2RlIGdvZXMgaGVyZQoJZm9yIChpbnQgaSA9IDA7IGkgPCBNT1RPUlZFTF9ISVNUT1JZX0xFTkdUSCArIDI7IGkrKykKCXsKCQlwb3NpdGlvbiA9IGk7CgkJUElEX1J1blVwZGF0ZU1vdG9yVmVsKCk7CgkJcHJpbnRmKCJNb3RvciBWZWw9JWRcbiIsIG1vdG9yVmVsKTsKCX0KCXJldHVybiAwOwp9Cg==