ws2812-achterbahn/wagon.cpp

178 lines
4.1 KiB
C++
Raw Normal View History

#include "wagon.h"
Wagon::Wagon(int id,int numpixels, Adafruit_NeoPixel *strip,uint8_t *height,float pos, float wagonlength,float startvel,float startacc)
{
_id = id;
_numpixels=numpixels;
_pos = pos;
_wagonlength = wagonlength;
_strip=strip;
_height=height;
_vel=startvel;
_acc=startacc;
2018-04-08 17:47:34 +00:00
_spawntime=millis();
}
bool Wagon::operator==(const Wagon &r) const {
return (r._id == _id);
}
void Wagon::updatePhysics(float updatedelayms)
{
/*
float acceleration=0;
for (int cpos=int(_pos);cpos>int(_pos-_wagonlength);cpos--){
acceleration=(_height[(int)cpos]-_height[(int)cpos+1])*0.03;
}
acceleration/=int(_wagonlength);
_vel+= acceleration; //Acceleration from height difference
_vel*=1-0.001; //resistance
float airresistance=_vel*_vel *0.005;//air resistance
if (_vel>=0){
_vel-=airresistance;
}else{
_vel+=airresistance;
}*/
#define CONST_G 9.81
#define PIXELDISTANCE 1.6666667 // 1/60.0 * 100
#define C_ROLL 0.001 // = Croll * G https://de.wikipedia.org/wiki/Rollwiderstand 0.001
#define AIRRES 0.18 //C_w*A*0.5*rho Air resistance: C_w * A * 0.5 * rho (.... *v^2)
//http://www.kfz-tech.de/Biblio/Formelsammlung/Luftwiderstand.htm C_w 0.6
//rho Massendichte luft 1,2041
//A = 1m^2
2018-04-08 17:47:34 +00:00
float m=40/_wagonlength; //mass of part of a wagon
_acc=0;
int cpos=(int)_pos;
for (int cpos=(int)_pos;cpos>(int)(_pos-_wagonlength);cpos--){ //for each wagon
float hdiff=getHeight((int) (cpos-0.5)) - getHeight((int)(cpos+0.5));
//Serial.print("hdiff=");
//Serial.print(hdiff);
float beta=atan2(PIXELDISTANCE, hdiff);
//Serial.print(" beta=");
//Serial.println(beta);
//_acc += CONST_G * cos(beta) - C_ROLLG*sin(beta) - AIRRES/m*_vel*_vel;
float aa=CONST_G * cos(beta) *updatedelayms/1000; //Gravity and m/s^2 time correction
float bb=C_ROLL*CONST_G*updatedelayms/1000*sin(beta); //roll resistance
if (_vel<0){
bb*=-1;
}
2018-04-08 17:47:34 +00:00
float cc=AIRRES/m*pow(_vel,2); //air resistance
if (_vel<0){
cc*=-1;
}
//Serial.print("aa="); Serial.print(aa);
//Serial.print(" bb="); Serial.print(bb);
//Serial.print(" cc="); Serial.println(cc);
_acc += aa - bb - cc;
}
_acc*=updatedelayms/1000;
2018-04-08 17:47:34 +00:00
_vel += _acc;
_pos += _vel/PIXELDISTANCE;
2018-04-08 17:47:34 +00:00
Serial.print(" Vel=");
Serial.print(_vel);
Serial.print(" Acc=");
2018-04-08 17:47:34 +00:00
Serial.println(_acc);
}
float Wagon::getHeight(int p){
if (p<0){
2018-04-08 17:47:34 +00:00
//p=0;
return (-p)*10;
}else if(p>=_numpixels){
2018-04-08 17:47:34 +00:00
//p=_numpixels-1;
return (p-_numpixels)*10;
}
return _height[p];
}
void Wagon::updateGraphics()
{
2018-04-08 17:47:34 +00:00
float wagonfeathering=2;
for(int i=_pos+wagonfeathering;i>_pos-_wagonlength-wagonfeathering;i--){
float featherbrightness=1;
if (i>_pos){ //in front of wagon
featherbrightness=1 - (i-_pos)/wagonfeathering;
}else if (i<_pos-_wagonlength){ //behind of wagon
featherbrightness=1 - (_pos-_wagonlength -i)/wagonfeathering;
}
if (featherbrightness<=0){ //distpercent between 0 and 1. 1-> full brightness, 0-> feathering distance away
featherbrightness=0;
}
2018-04-08 17:47:34 +00:00
//uint32_t c=_strip->Color(0,255*featherbrightness,0);
//uint32_t c=Wheel(_height[i]/45.0*255,featherbrightness);
//uint8_t b=_height[i]*255/45;
uint8_t b=abs(_vel)*255.0;
uint32_t c=_strip->Color(b*featherbrightness,(255-b)*featherbrightness,0);
_strip->setPixelColor(i,c);
}
}
int Wagon::pos()
{
return _pos;
}
int Wagon::id()
{
return _id;
}
2018-04-08 17:47:34 +00:00
long Wagon::spawntime()
{
return _spawntime;
}
bool Wagon::alive()
{
2018-04-08 17:47:34 +00:00
/*if (_pos>_numpixels){
return false;
}*/
if (millis()>_spawntime+30*1000){ //too old
return false;
}
2018-04-08 17:47:34 +00:00
return true;
}
2018-04-08 17:47:34 +00:00
uint32_t Wagon::Wheel(byte WheelPos,float brightness) {
WheelPos = 255 - WheelPos;
if(WheelPos < 85) {
return _strip->Color(255 - WheelPos * 3*brightness, 0, WheelPos * 3*brightness);
}
if(WheelPos < 170) {
WheelPos -= 85;
return _strip->Color(0, WheelPos * 3*brightness, 255 - WheelPos * 3*brightness);
}
WheelPos -= 170;
return _strip->Color(WheelPos * 3*brightness, 255 - WheelPos * 3*brightness, 0);
}