inital commit, working roller coaster based ony physics
This commit is contained in:
commit
153a9f0d83
|
@ -0,0 +1,161 @@
|
|||
#include <Adafruit_NeoPixel.h>
|
||||
#ifdef __AVR__
|
||||
#include <avr/power.h>
|
||||
#endif
|
||||
|
||||
#include <vector>
|
||||
#include "wagon.h"
|
||||
|
||||
#define PIN D2
|
||||
#define NUMPIXELS 300
|
||||
|
||||
Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
|
||||
|
||||
long lastPixelUpdate=0;
|
||||
#define PIXELUPDATETIME 10
|
||||
long lastRoutineUpdate=0;
|
||||
#define ROUTINEUPDATETIME 10
|
||||
|
||||
long loopmillis=0;
|
||||
|
||||
uint8_t height[NUMPIXELS];
|
||||
|
||||
std::vector <Wagon> wagon_arr;
|
||||
uint8_t maxid=0;
|
||||
|
||||
|
||||
void setup() {
|
||||
Serial.begin(115200);
|
||||
|
||||
for (int i=0;i<NUMPIXELS;i++){
|
||||
height[i]=0;
|
||||
}
|
||||
//Temporaer
|
||||
int h=0;
|
||||
for (int i=2;i<=30;i++){
|
||||
height[i]=h*43/(30-2);
|
||||
h++;
|
||||
}
|
||||
for (int i=31;i<(31+55);i++){ //gerade obene ebene
|
||||
height[i]=43;
|
||||
}
|
||||
h=30;
|
||||
for (int i=(31+55);i<(31+55+30);i++){
|
||||
height[i]=h*43/(30);
|
||||
h--;
|
||||
}
|
||||
|
||||
h=0;
|
||||
for (int i=(31+55+30+40);i<=(31+55+30+40+15);i++){
|
||||
height[i]=h*23/(15);
|
||||
h++;
|
||||
}
|
||||
for (int i=(31+55+30+40+15);i<=(31+55+30+40+15+55);i++){ //gerade mittlere ebene
|
||||
height[i]=23;
|
||||
}
|
||||
|
||||
h=20;
|
||||
for (int i=(31+55+30+40+15+55);i<=(31+55+30+40+15+55+20);i++){ //gerade mittlere ebene
|
||||
height[i]=h*23/20;
|
||||
h--;
|
||||
}
|
||||
|
||||
|
||||
h=0;
|
||||
for (int i=NUMPIXELS-30;i<NUMPIXELS;i++){
|
||||
height[i]=h*40/30;
|
||||
h++;
|
||||
}
|
||||
|
||||
|
||||
|
||||
strip.begin();
|
||||
strip.setBrightness(150);
|
||||
strip.show(); // Initialize all pixels to 'off'
|
||||
Serial.println("Started");
|
||||
|
||||
previewHeightmap();
|
||||
|
||||
spawnWagon();
|
||||
|
||||
}
|
||||
|
||||
void previewHeightmap(){
|
||||
int maxheight=45;
|
||||
for (int i=0;i<NUMPIXELS;i++){
|
||||
|
||||
//uint32_t c=Wheel(height[i]*255/45);
|
||||
uint8_t b=height[i]*255/45;
|
||||
uint32_t c=strip.Color(255-b,b,0);
|
||||
strip.setPixelColor(i,c);
|
||||
}
|
||||
strip.show();
|
||||
delay(3000);
|
||||
}
|
||||
|
||||
void spawnWagon(){
|
||||
Wagon tmpr = Wagon(maxid++,NUMPIXELS,&strip, height, 35, 6, 0.5,0); //spawn new wagon
|
||||
wagon_arr.push_back(tmpr);
|
||||
Serial.println("Spawned Wagon");
|
||||
}
|
||||
|
||||
void loop() {
|
||||
loopmillis=millis();
|
||||
|
||||
//######################### Update LED Output
|
||||
if (lastPixelUpdate+PIXELUPDATETIME<loopmillis){
|
||||
lastPixelUpdate=loopmillis;
|
||||
|
||||
for (int i=0;i<NUMPIXELS;i++){ //all black
|
||||
uint32_t c=strip.Color(0,0,0);
|
||||
strip.setPixelColor(i,c);
|
||||
}
|
||||
|
||||
for (std::vector<Wagon>::iterator it = wagon_arr.begin(); it != wagon_arr.end(); ++it) //all wagons
|
||||
{
|
||||
Wagon & w = *it;
|
||||
w.updateGraphics();
|
||||
}
|
||||
|
||||
strip.show();
|
||||
}
|
||||
|
||||
|
||||
if (lastRoutineUpdate+ROUTINEUPDATETIME<loopmillis-ROUTINEUPDATETIME){
|
||||
Serial.println("Behind!!!!!!!!!!");
|
||||
}
|
||||
//######################### Update Physics
|
||||
if (lastRoutineUpdate+ROUTINEUPDATETIME<loopmillis){
|
||||
lastRoutineUpdate=loopmillis;
|
||||
|
||||
for (std::vector<Wagon>::iterator it = wagon_arr.begin(); it != wagon_arr.end(); ++it) //all wagons
|
||||
{
|
||||
Wagon & w = *it;
|
||||
w.updatePhysics(ROUTINEUPDATETIME);
|
||||
if (!w.alive())
|
||||
{
|
||||
it = wagon_arr.erase(it); // After erasing, it is now pointing the next element.
|
||||
--it;
|
||||
spawnWagon(); //spawn new one
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Input a value 0 to 255 to get a color value.
|
||||
// The colours are a transition r - g - b - back to r.
|
||||
uint32_t Wheel(byte WheelPos) {
|
||||
WheelPos = 255 - WheelPos;
|
||||
if(WheelPos < 85) {
|
||||
return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
|
||||
}
|
||||
if(WheelPos < 170) {
|
||||
WheelPos -= 85;
|
||||
return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
|
||||
}
|
||||
WheelPos -= 170;
|
||||
return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
|
||||
}
|
|
@ -0,0 +1,134 @@
|
|||
#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;
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
float m=50/_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;
|
||||
}
|
||||
float cc=AIRRES/m*_vel*_vel; //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;
|
||||
|
||||
_vel += _acc;
|
||||
_pos += _vel/PIXELDISTANCE;
|
||||
/*Serial.print(" Vel=");
|
||||
Serial.print(_vel);
|
||||
Serial.print(" Acc=");
|
||||
Serial.println(_acc);*/
|
||||
}
|
||||
|
||||
float Wagon::getHeight(int p){
|
||||
if (p<0){
|
||||
p=0;
|
||||
}else if(p>=_numpixels){
|
||||
p=_numpixels-1;
|
||||
}
|
||||
return _height[p];
|
||||
}
|
||||
|
||||
void Wagon::updateGraphics()
|
||||
{
|
||||
for(int i=_pos;i>_pos-_wagonlength;i--){
|
||||
uint32_t c=_strip->Color(0,255,0);
|
||||
if (i==int(_pos)){
|
||||
c=_strip->Color(0,255,100);
|
||||
}
|
||||
_strip->setPixelColor(i,c);
|
||||
}
|
||||
//_strip->setPixelColor(10,_strip->Color(255,0,0));
|
||||
}
|
||||
|
||||
int Wagon::pos()
|
||||
{
|
||||
return _pos;
|
||||
}
|
||||
|
||||
int Wagon::id()
|
||||
{
|
||||
return _id;
|
||||
}
|
||||
|
||||
bool Wagon::alive()
|
||||
{
|
||||
if (_pos>_numpixels){
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
#ifndef WAGON_H
|
||||
#define WAGON_H
|
||||
#include <Adafruit_NeoPixel.h>
|
||||
#include <math.h>
|
||||
|
||||
class Wagon
|
||||
{
|
||||
public:
|
||||
int _id;
|
||||
Wagon(int id,int numpixels,Adafruit_NeoPixel *strip,uint8_t *height, float pos, float wagonlength, float startvel,float startacc);
|
||||
Wagon();
|
||||
bool operator==(const Wagon &r) const;
|
||||
void updatePhysics(float updatedelayms);
|
||||
void updateGraphics();
|
||||
float getHeight(int p);
|
||||
int pos();
|
||||
int id();
|
||||
bool alive();
|
||||
private:
|
||||
int _numpixels;
|
||||
Adafruit_NeoPixel *_strip;
|
||||
float _pos;
|
||||
float _vel;
|
||||
float _acc;
|
||||
float _wagonlength;
|
||||
uint8_t *_height;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
Loading…
Reference in New Issue